Skip to content

Commit 462099d

Browse files
juelgjuelg
committed
added joint controller
1 parent 7d43df4 commit 462099d

File tree

5 files changed

+217
-13
lines changed

5 files changed

+217
-13
lines changed

‎python/rcsss/_core/hw/__init__.pyi‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,9 @@ class FHState(rcsss._core.common.GState):
4949
class FR3(rcsss._core.common.Robot):
5050
def __init__(self, ip: str, ik: rcsss._core.common.IK | None = None) -> None: ...
5151
def automatic_error_recovery(self) -> None: ...
52+
def controller_set_joint_position(
53+
self, desired_q: numpy.ndarray[typing.Literal[7], numpy.dtype[numpy.float64]]
54+
) -> None: ...
5255
def double_tap_robot_to_continue(self) -> None: ...
5356
def get_parameters(self) -> FR3Config: ...
5457
def get_state(self) -> FR3State: ...

‎src/common/LinearPoseTrajInterpolator.h‎

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,77 @@ class LinearPoseTrajInterpolator {
9595
q_t = last_q_t_;
9696
};
9797
};
98+
class LinearJointPositionTrajInterpolator {
99+
private:
100+
using Vector7d = Eigen::Matrix<double, 7, 1>;
101+
using Vector7i = Eigen::Matrix<int, 7, 1>;
102+
103+
Vector7d q_start_;
104+
Vector7d q_goal_;
105+
106+
Vector7d last_q_t_;
107+
Vector7d prev_q_goal_;
108+
109+
double dt_;
110+
double last_time_;
111+
double max_time_;
112+
double start_time_;
113+
bool start_;
114+
bool first_goal_;
115+
116+
double interpolation_fraction_; // fraction of actual interpolation within an
117+
// interval
118+
119+
public:
120+
inline LinearJointPositionTrajInterpolator()
121+
: dt_(0.), last_time_(0.), max_time_(1.), start_time_(0.), start_(false),
122+
first_goal_(true){};
123+
124+
inline ~LinearJointPositionTrajInterpolator(){};
125+
126+
inline void Reset(const double &time_sec,
127+
const Eigen::Matrix<double, 7, 1> &q_start,
128+
const Eigen::Matrix<double, 7, 1> &q_goal,
129+
const int &policy_rate, const int &rate,
130+
const double &traj_interpolator_time_fraction) {
131+
dt_ = 1. / static_cast<double>(rate);
132+
last_time_ = time_sec;
133+
134+
max_time_ =
135+
1. / static_cast<double>(policy_rate) * traj_interpolator_time_fraction;
136+
start_time_ = time_sec;
137+
138+
start_ = false;
139+
140+
if (first_goal_) {
141+
q_start_ = q_start;
142+
prev_q_goal_ = q_start;
143+
first_goal_ = false;
144+
// std::cout << "First goal of the interpolation" << std::endl;
145+
} else {
146+
prev_q_goal_ = q_goal_;
147+
q_start_ = prev_q_goal_;
148+
}
149+
q_goal_ = q_goal;
150+
};
151+
152+
inline void GetNextStep(const double &time_sec, Vector7d &q_t) {
153+
if (!start_) {
154+
start_time_ = time_sec;
155+
last_q_t_ = q_start_;
156+
start_ = true;
157+
}
158+
// std::cout << q_start_.transpose() << " | " << q_goal_.transpose() <<
159+
// std::endl;
160+
if (last_time_ + dt_ <= time_sec) {
161+
double t =
162+
std::min(std::max((time_sec - start_time_) / max_time_, 0.), 1.);
163+
last_q_t_ = q_start_ + t * (q_goal_ - q_start_);
164+
last_time_ = time_sec;
165+
}
166+
q_t = last_q_t_;
167+
};
168+
};
98169
} // namespace common
99170
} // namespace rcs
100171
#endif // DEOXYS_FRANKA_INTERFACE_INCLUDE_UTILS_TRAJ_INTERPOLATORS_POSE_TRAJ_INTERPOLATOR_H_

‎src/hw/FR3.cpp‎

Lines changed: 134 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -140,11 +140,41 @@ void TorqueSafetyGuardFn(std::array<double, 7> &tau_d_array, double min_torque,
140140
}
141141
}
142142

143-
void FR3::osc2_set_cartesian_position(
144-
const common::Pose &desired_pose_EE_in_base_frame) {
145143

146-
// auto pose = this->get_cartesian_position();
144+
void FR3::controller_set_joint_position(
145+
const common::Vector7d &desired_q) {
146+
147+
// from deoxys/config/osc-position-controller.yml
148+
double traj_interpolation_time_fraction = 0.3; // in s
149+
// form deoxys/config/charmander.yml
150+
int policy_rate = 20;
151+
int traj_rate = 500;
152+
153+
if (!this->control_thread_running) {
154+
this->controller_time = 0.0;
155+
this->curr_joint_position = this->get_joint_position();
156+
} else {
157+
this->joint_interpolator_mutex.lock();
158+
}
159+
160+
this->joint_interpolator.Reset(
161+
this->controller_time, this->curr_joint_position, desired_q, policy_rate, traj_rate,
162+
traj_interpolation_time_fraction);
163+
164+
// if not thread is running, then start
165+
if (!this->control_thread_running) {
166+
this->control_thread_running = true;
167+
this->control_thread = std::thread(&FR3::joint_controller, this);
168+
} else {
169+
this->joint_interpolator_mutex.unlock();
170+
}
171+
}
147172

173+
// todos
174+
// - controller type
175+
// - joint type
176+
void FR3::osc2_set_cartesian_position(
177+
const common::Pose &desired_pose_EE_in_base_frame) {
148178
// from deoxys/config/osc-position-controller.yml
149179
double traj_interpolation_time_fraction = 0.3;
150180
// form deoxys/config/charmander.yml
@@ -159,9 +189,10 @@ void FR3::osc2_set_cartesian_position(
159189
}
160190

161191
this->traj_interpolator.Reset(
162-
this->controller_time, this->curr_pose.translation(), this->curr_pose.quaternion(),
163-
desired_pose_EE_in_base_frame.translation(), desired_pose_EE_in_base_frame.quaternion(),
164-
policy_rate, traj_rate, traj_interpolation_time_fraction);
192+
this->controller_time, this->curr_pose.translation(),
193+
this->curr_pose.quaternion(), desired_pose_EE_in_base_frame.translation(),
194+
desired_pose_EE_in_base_frame.quaternion(), policy_rate, traj_rate,
195+
traj_interpolation_time_fraction);
165196

166197
// if not thread is running, then start
167198
if (!this->control_thread_running) {
@@ -195,9 +226,10 @@ void FR3::osc_set_cartesian_position(
195226
}
196227

197228
this->traj_interpolator.Reset(
198-
this->controller_time, this->curr_pose.translation(), this->curr_pose.quaternion(),
199-
desired_pos_EE_in_base_frame, fixed_desired_quat_EE_in_base_frame,
200-
policy_rate, traj_rate, traj_interpolation_time_fraction);
229+
this->controller_time, this->curr_pose.translation(),
230+
this->curr_pose.quaternion(), desired_pos_EE_in_base_frame,
231+
fixed_desired_quat_EE_in_base_frame, policy_rate, traj_rate,
232+
traj_interpolation_time_fraction);
201233

202234
// if not thread is running, then start
203235
if (!this->control_thread_running) {
@@ -481,7 +513,6 @@ void FR3::osc2() {
481513
// The manual residual mass matrix to add on the internal mass matrix
482514
residual_mass_vec_ << 0.0, 0.0, 0.0, 0.0, 0.1, 0.5, 0.5;
483515

484-
485516
joint_max_ << 2.8978, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973;
486517
joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973;
487518
avoidance_weights_ << 1., 1., 1., 1., 1., 10., 10.;
@@ -493,6 +524,7 @@ void FR3::osc2() {
493524

494525
// torques handler
495526
if (!this->control_thread_running) {
527+
// TODO: test if this also works when the robot is moving
496528
franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
497529
return franka::MotionFinished(zero_torques);
498530
}
@@ -512,7 +544,6 @@ void FR3::osc2() {
512544
int policy_rate = 20;
513545
int traj_rate = 500;
514546

515-
516547
this->traj_interpolator_mutex.lock();
517548
// if (this->controller_time == 0) {
518549
// this->traj_interpolator.Reset(
@@ -565,7 +596,6 @@ void FR3::osc2() {
565596
// Joint velocity
566597
Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
567598

568-
569599
if (desired_quat_EE_in_base_frame.coeffs().dot(
570600
quat_EE_in_base_frame.coeffs()) < 0.0) {
571601
quat_EE_in_base_frame.coeffs() << -quat_EE_in_base_frame.coeffs();
@@ -660,6 +690,98 @@ void FR3::osc2() {
660690
});
661691
}
662692

693+
void FR3::joint_controller() {
694+
franka::Model model = this->robot.loadModel();
695+
this->controller_time = 0.0;
696+
this->robot.setCollisionBehavior(
697+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
698+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
699+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
700+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
701+
702+
703+
// deoxys/config/joint-impedance-controller.yml
704+
common::Vector7d Kp;
705+
Kp << 100., 100., 100., 100., 75., 150., 50.;
706+
707+
common::Vector7d Kd;
708+
Kd << 20., 20., 20., 20., 7.5, 15.0, 5.0;
709+
710+
711+
Eigen::Array<double, 7, 1> joint_max_;
712+
Eigen::Array<double, 7, 1> joint_min_;
713+
714+
715+
joint_max_ << 2.8978, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973;
716+
joint_min_ << -2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973;
717+
718+
this->robot.control([&](const franka::RobotState &robot_state,
719+
franka::Duration period) -> franka::Torques {
720+
std::chrono::high_resolution_clock::time_point t1 =
721+
std::chrono::high_resolution_clock::now();
722+
723+
724+
// torques handler
725+
if (!this->control_thread_running) {
726+
// TODO: test if this also works when the robot is moving
727+
franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
728+
return franka::MotionFinished(zero_torques);
729+
}
730+
731+
732+
common::Vector7d desired_q;
733+
734+
common::Pose pose(robot_state.O_T_EE);
735+
736+
this->joint_interpolator_mutex.lock();
737+
this->controller_time += period.toSec();
738+
this->joint_interpolator.GetNextStep(this->controller_time,
739+
desired_q);
740+
this->joint_interpolator_mutex.unlock();
741+
// end torques handler
742+
743+
Eigen::Matrix<double, 7, 1> tau_d;
744+
745+
std::array<double, 49> mass_array = model.mass(robot_state);
746+
Eigen::Map<Eigen::Matrix<double, 7, 7>> M(mass_array.data());
747+
748+
// coriolis and gravity
749+
std::array<double, 7> coriolis_array = model.coriolis(robot_state);
750+
Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(
751+
coriolis_array.data());
752+
753+
// Current joint velocity
754+
Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
755+
756+
// Current joint position
757+
Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
758+
759+
Eigen::MatrixXd joint_pos_error(7, 1);
760+
761+
Eigen::Matrix<double, 7, 1> current_q, current_dq;
762+
763+
joint_pos_error << desired_q - q;
764+
765+
tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(current_dq);
766+
767+
Eigen::Matrix<double, 7, 1> dist2joint_max;
768+
Eigen::Matrix<double, 7, 1> dist2joint_min;
769+
770+
dist2joint_max = joint_max_.matrix() - current_q;
771+
dist2joint_min = current_q - joint_min_.matrix();
772+
773+
for (int i = 0; i < 7; i++) {
774+
if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.;
775+
if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.;
776+
}
777+
778+
std::array<double, 7> tau_d_array{};
779+
Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
780+
781+
return tau_d_array;
782+
});
783+
}
784+
663785
void FR3::zero_torque() {
664786
// start thread
665787

‎src/hw/FR3.h‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,12 @@ class FR3 : public common::Robot {
5555
common::LinearPoseTrajInterpolator traj_interpolator;
5656
std::mutex traj_interpolator_mutex;
5757
double controller_time = 0.0;
58-
common::Pose curr_pose = common::Pose::Identity();
58+
common::Pose curr_pose;
59+
60+
61+
common::LinearJointPositionTrajInterpolator joint_interpolator;
62+
std::mutex joint_interpolator_mutex;
63+
common::Vector7d curr_joint_position;
5964

6065
bool control_thread_running = false;
6166

@@ -81,13 +86,15 @@ class FR3 : public common::Robot {
8186

8287
void set_guiding_mode(bool enabled);
8388

89+
void controller_set_joint_position(const common::Vector7d &desired_q);
8490
void osc_set_cartesian_position(const Eigen::Vector3d &desired_pos_EE_in_base_frame);
8591
void osc2_set_cartesian_position(const common::Pose &desired_pose_EE_in_base_frame);
8692

8793
void stop_control_thread();
8894

8995
void osc();
9096
void osc2();
97+
void joint_controller();
9198

9299
void zero_torque();
93100

‎src/pybind/rcsss.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -363,6 +363,7 @@ PYBIND11_MODULE(_core, m) {
363363
py::arg("enabled"))
364364
.def("zero_torque", &rcs::hw::FR3::zero_torque)
365365
.def("osc_set_cartesian_position", &rcs::hw::FR3::osc_set_cartesian_position, py::arg("desired_pos_EE_in_base_frame"))
366+
.def("controller_set_joint_position", &rcs::hw::FR3::controller_set_joint_position, py::arg("desired_q"))
366367
.def("osc2_set_cartesian_position", &rcs::hw::FR3::osc2_set_cartesian_position, py::arg("desired_pose_EE_in_base_frame"))
367368
.def("stop_control_thread", &rcs::hw::FR3::stop_control_thread)
368369
.def("automatic_error_recovery", &rcs::hw::FR3::automatic_error_recovery)

0 commit comments

Comments
 (0)