Skip to content

Commit aec9e76

Browse files
juelgjuelg
authored
Merge pull request #321 from RobotControlStack/juelg/feat-pd-modes
feat(franka): pd coefficients in config
2 parents 162c774 + cd9c954 commit aec9e76

5 files changed

Lines changed: 74 additions & 24 deletions

File tree

‎extensions/rcs_fr3/src/hw/Franka.cpp‎

Lines changed: 16 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -163,13 +163,13 @@ void PInverse(const Eigen::MatrixXd& M, Eigen::MatrixXd& M_inv,
163163
M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose());
164164
}
165165

166-
void TorqueSafetyGuardFn(std::array<double, 7>& tau_d_array, double min_torque,
167-
double max_torque) {
166+
void TorqueSafetyGuardFn(std::array<double, 7>& tau_d_array,
167+
const std::array<double, 7>& torque_limit) {
168168
for (size_t i = 0; i < tau_d_array.size(); i++) {
169-
if (tau_d_array[i] < min_torque) {
170-
tau_d_array[i] = min_torque;
171-
} else if (tau_d_array[i] > max_torque) {
172-
tau_d_array[i] = max_torque;
169+
if (tau_d_array[i] < -torque_limit[i]) {
170+
tau_d_array[i] = -torque_limit[i];
171+
} else if (tau_d_array[i] > torque_limit[i]) {
172+
tau_d_array[i] = torque_limit[i];
173173
}
174174
}
175175
}
@@ -282,6 +282,8 @@ void Franka::stop_control_thread() {
282282

283283
void Franka::osc() {
284284
franka::Model model = this->robot.loadModel();
285+
const Eigen::Vector3d kp_p_cfg = this->m_cfg.kp_p;
286+
const double kp_r_cfg = this->m_cfg.kp_r;
285287

286288
this->controller_time = 0.0;
287289

@@ -314,9 +316,8 @@ void Franka::osc() {
314316
Eigen::Array<double, 7, 1> joint_min_;
315317
Eigen::Array<double, 7, 1> avoidance_weights_;
316318

317-
// values from deoxys/config/osc-position-controller.yml
318-
Kp_p.diagonal() << 150, 150, 150;
319-
Kp_r.diagonal() << 250, 250, 250;
319+
Kp_p.diagonal() << kp_p_cfg;
320+
Kp_r.diagonal() << kp_r_cfg, kp_r_cfg, kp_r_cfg;
320321

321322
Kd_p << Kp_p.cwiseSqrt() * 2.0;
322323
Kd_r << Kp_r.cwiseSqrt() * 2.0;
@@ -495,9 +496,8 @@ void Franka::osc() {
495496
franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);
496497

497498
// deoxys/config/control_config.yml
498-
double min_torque = -5;
499-
double max_torque = 5;
500-
TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque);
499+
std::array<double, 7> torque_limit = {5, 5, 5, 5, 5, 5, 5};
500+
TorqueSafetyGuardFn(tau_d_rate_limited, torque_limit);
501501

502502
return tau_d_rate_limited;
503503
});
@@ -512,6 +512,8 @@ void Franka::osc() {
512512

513513
void Franka::joint_controller() {
514514
franka::Model model = this->robot.loadModel();
515+
const common::Vector7d Kp = this->m_cfg.kp;
516+
const common::Vector7d Kd = this->m_cfg.kd;
515517
this->controller_time = 0.0;
516518

517519
// conservative collision and impedance behavior
@@ -524,13 +526,6 @@ void Franka::joint_controller() {
524526
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
525527
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
526528

527-
// deoxys/config/joint-impedance-controller.yml
528-
common::Vector7d Kp;
529-
Kp << 100., 100., 100., 100., 75., 150., 50.;
530-
531-
common::Vector7d Kd;
532-
Kd << 20., 20., 20., 20., 7.5, 15.0, 5.0;
533-
534529
Eigen::Array<double, 7, 1> joint_max_;
535530
Eigen::Array<double, 7, 1> joint_min_;
536531

@@ -595,9 +590,8 @@ void Franka::joint_controller() {
595590
franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);
596591

597592
// deoxys/config/control_config.yml
598-
double min_torque = -5;
599-
double max_torque = 5;
600-
TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque);
593+
std::array<double, 7> torque_limit = {5, 5, 5, 5, 5, 5, 5};
594+
TorqueSafetyGuardFn(tau_d_rate_limited, torque_limit);
601595

602596
return tau_d_rate_limited;
603597
});

‎extensions/rcs_fr3/src/hw/Franka.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,14 @@ struct FrankaConfig : common::RobotConfig {
3838
common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE;
3939
IKSolver ik_solver = IKSolver::rcs_ik;
4040
double speed_factor = DEFAULT_SPEED_FACTOR;
41+
// deoxys/config/joint-impedance-controller.yml
42+
common::Vector7d kp =
43+
(common::Vector7d() << 100., 100., 100., 100., 75., 150., 50.).finished();
44+
common::Vector7d kd =
45+
(common::Vector7d() << 20., 20., 20., 20., 7.5, 15.0, 5.0).finished();
46+
// values from deoxys/config/osc-position-controller.yml
47+
Eigen::Vector3d kp_p = (Eigen::Vector3d() << 150., 150., 150.).finished();
48+
double kp_r = 250.0;
4149
std::optional<FrankaLoad> load_parameters = std::nullopt;
4250
std::optional<common::Pose> nominal_end_effector_frame = std::nullopt;
4351
std::optional<common::Pose> world_to_robot = std::nullopt;

‎extensions/rcs_fr3/src/pybind/rcs.cpp‎

Lines changed: 26 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,10 @@ PYBIND11_MODULE(_core, m) {
128128
py::class_<rcs::hw::FrankaConfig>(hw, "FrankaConfig", robot_config)
129129
.def_readwrite("ik_solver", &rcs::hw::FrankaConfig::ik_solver)
130130
.def_readwrite("speed_factor", &rcs::hw::FrankaConfig::speed_factor)
131+
.def_readwrite("kp", &rcs::hw::FrankaConfig::kp)
132+
.def_readwrite("kd", &rcs::hw::FrankaConfig::kd)
133+
.def_readwrite("kp_p", &rcs::hw::FrankaConfig::kp_p)
134+
.def_readwrite("kp_r", &rcs::hw::FrankaConfig::kp_r)
131135
.def_readwrite("load_parameters", &rcs::hw::FrankaConfig::load_parameters)
132136
.def_readwrite("nominal_end_effector_frame",
133137
&rcs::hw::FrankaConfig::nominal_end_effector_frame)
@@ -142,7 +146,9 @@ PYBIND11_MODULE(_core, m) {
142146
py::class_<rcs::hw::FR3Config, rcs::hw::FrankaConfig>(hw, "FR3Config")
143147
.def(py::init(
144148
[](const std::string& ip, rcs::hw::IKSolver ik_solver,
145-
double speed_factor,
149+
double speed_factor, const rcs::common::Vector7d& kp,
150+
const rcs::common::Vector7d& kd, const Eigen::Vector3d& kp_p,
151+
double kp_r,
146152
std::optional<rcs::hw::FrankaLoad> load_parameters,
147153
std::optional<rcs::common::Pose> nominal_end_effector_frame,
148154
std::optional<rcs::common::Pose> world_to_robot,
@@ -153,6 +159,10 @@ PYBIND11_MODULE(_core, m) {
153159
rcs::hw::FR3Config cfg;
154160
cfg.ik_solver = ik_solver;
155161
cfg.speed_factor = speed_factor;
162+
cfg.kp = kp;
163+
cfg.kd = kd;
164+
cfg.kp_p = kp_p;
165+
cfg.kp_r = kp_r;
156166
cfg.load_parameters = load_parameters;
157167
cfg.nominal_end_effector_frame = nominal_end_effector_frame;
158168
cfg.world_to_robot = world_to_robot;
@@ -168,6 +178,10 @@ PYBIND11_MODULE(_core, m) {
168178
}),
169179
py::arg("ip"), py::arg("ik_solver") = default_fr3_config.ik_solver,
170180
py::arg("speed_factor") = default_fr3_config.speed_factor,
181+
py::arg("kp") = default_fr3_config.kp,
182+
py::arg("kd") = default_fr3_config.kd,
183+
py::arg("kp_p") = default_fr3_config.kp_p,
184+
py::arg("kp_r") = default_fr3_config.kp_r,
171185
py::arg("load_parameters") = default_fr3_config.load_parameters,
172186
py::arg("nominal_end_effector_frame") =
173187
default_fr3_config.nominal_end_effector_frame,
@@ -184,7 +198,9 @@ PYBIND11_MODULE(_core, m) {
184198
py::class_<rcs::hw::PandaConfig, rcs::hw::FrankaConfig>(hw, "PandaConfig")
185199
.def(py::init(
186200
[](const std::string& ip, rcs::hw::IKSolver ik_solver,
187-
double speed_factor,
201+
double speed_factor, const rcs::common::Vector7d& kp,
202+
const rcs::common::Vector7d& kd, const Eigen::Vector3d& kp_p,
203+
double kp_r,
188204
std::optional<rcs::hw::FrankaLoad> load_parameters,
189205
std::optional<rcs::common::Pose> nominal_end_effector_frame,
190206
std::optional<rcs::common::Pose> world_to_robot,
@@ -195,6 +211,10 @@ PYBIND11_MODULE(_core, m) {
195211
rcs::hw::PandaConfig cfg;
196212
cfg.ik_solver = ik_solver;
197213
cfg.speed_factor = speed_factor;
214+
cfg.kp = kp;
215+
cfg.kd = kd;
216+
cfg.kp_p = kp_p;
217+
cfg.kp_r = kp_r;
198218
cfg.load_parameters = load_parameters;
199219
cfg.nominal_end_effector_frame = nominal_end_effector_frame;
200220
cfg.world_to_robot = world_to_robot;
@@ -210,6 +230,10 @@ PYBIND11_MODULE(_core, m) {
210230
}),
211231
py::arg("ip"), py::arg("ik_solver") = default_panda_config.ik_solver,
212232
py::arg("speed_factor") = default_panda_config.speed_factor,
233+
py::arg("kp") = default_panda_config.kp,
234+
py::arg("kd") = default_panda_config.kd,
235+
py::arg("kp_p") = default_panda_config.kp_p,
236+
py::arg("kp_r") = default_panda_config.kp_r,
213237
py::arg("load_parameters") = default_panda_config.load_parameters,
214238
py::arg("nominal_end_effector_frame") =
215239
default_panda_config.nominal_end_effector_frame,

‎extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,6 +107,10 @@ class FrankaConfig(rcs._core.common.RobotConfig):
107107
ignore_realtime: bool
108108
ik_solver: IKSolver
109109
ip: str
110+
kd: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
111+
kp: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
112+
kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]
113+
kp_r: float
110114
load_parameters: FrankaLoad | None
111115
nominal_end_effector_frame: rcs._core.common.Pose | None
112116
speed_factor: float
@@ -298,6 +302,10 @@ class FR3Config(FrankaConfig):
298302
ip: str,
299303
ik_solver: IKSolver = ...,
300304
speed_factor: float = 0.2,
305+
kp: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
306+
kd: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
307+
kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] = ...,
308+
kp_r: float = 250.0,
301309
load_parameters: FrankaLoad | None = None,
302310
nominal_end_effector_frame: rcs._core.common.Pose | None = None,
303311
world_to_robot: rcs._core.common.Pose | None = None,
@@ -315,6 +323,10 @@ class PandaConfig(FrankaConfig):
315323
ip: str,
316324
ik_solver: IKSolver = ...,
317325
speed_factor: float = 0.2,
326+
kp: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
327+
kd: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
328+
kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] = ...,
329+
kp_r: float = 250.0,
318330
load_parameters: FrankaLoad | None = None,
319331
nominal_end_effector_frame: rcs._core.common.Pose | None = None,
320332
world_to_robot: rcs._core.common.Pose | None = None,

‎extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,6 +107,10 @@ class FrankaConfig(rcs._core.common.RobotConfig):
107107
ignore_realtime: bool
108108
ik_solver: IKSolver
109109
ip: str
110+
kd: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
111+
kp: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
112+
kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]]
113+
kp_r: float
110114
load_parameters: FrankaLoad | None
111115
nominal_end_effector_frame: rcs._core.common.Pose | None
112116
speed_factor: float
@@ -298,6 +302,10 @@ class FR3Config(FrankaConfig):
298302
ip: str,
299303
ik_solver: IKSolver = ...,
300304
speed_factor: float = 0.2,
305+
kp: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
306+
kd: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
307+
kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] = ...,
308+
kp_r: float = 250.0,
301309
load_parameters: FrankaLoad | None = None,
302310
nominal_end_effector_frame: rcs._core.common.Pose | None = None,
303311
world_to_robot: rcs._core.common.Pose | None = None,
@@ -315,6 +323,10 @@ class PandaConfig(FrankaConfig):
315323
ip: str,
316324
ik_solver: IKSolver = ...,
317325
speed_factor: float = 0.2,
326+
kp: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
327+
kd: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
328+
kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] = ...,
329+
kp_r: float = 250.0,
318330
load_parameters: FrankaLoad | None = None,
319331
nominal_end_effector_frame: rcs._core.common.Pose | None = None,
320332
world_to_robot: rcs._core.common.Pose | None = None,

0 commit comments

Comments
 (0)