Skip to content

Commit e7bf1d2

Browse files
juelgjuelg
committed
feat(fr3/panda): config for collision values
1 parent c9e6f96 commit e7bf1d2

3 files changed

Lines changed: 42 additions & 22 deletions

File tree

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

Lines changed: 27 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -285,18 +285,21 @@ void Franka::osc() {
285285
const Eigen::Vector3d kp_p_cfg = this->m_cfg.kp_p;
286286
const double kp_r_cfg = this->m_cfg.kp_r;
287287
const common::Vector7d torque_limit = this->m_cfg.torque_limit;
288+
const bool allow_high_collision = this->m_cfg.allow_high_collision;
288289

289290
this->controller_time = 0.0;
290291

291292
// conservative collision and impedance behavior
292293
this->set_default_robot_behavior();
293294

294-
// high collision threshold values for high impedance
295-
this->robot.setCollisionBehavior(
296-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
297-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
298-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
299-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
295+
if (allow_high_collision) {
296+
// High collision threshold values for high impedance.
297+
this->robot.setCollisionBehavior(
298+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
299+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
300+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
301+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
302+
}
300303

301304
// from bench mark
302305
// ([150.0, 150.0, 60.0], 250.0), // kp_translation, kp_rotation
@@ -514,17 +517,20 @@ void Franka::joint_controller() {
514517
const common::Vector7d Kp = this->m_cfg.kp;
515518
const common::Vector7d Kd = this->m_cfg.kd;
516519
const common::Vector7d torque_limit = this->m_cfg.torque_limit;
520+
const bool allow_high_collision = this->m_cfg.allow_high_collision;
517521
this->controller_time = 0.0;
518522

519523
// conservative collision and impedance behavior
520524
this->set_default_robot_behavior();
521525

522-
// high collision threshold values for high impedance
523-
this->robot.setCollisionBehavior(
524-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
525-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
526-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
527-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
526+
if (allow_high_collision) {
527+
// High collision threshold values for high impedance.
528+
this->robot.setCollisionBehavior(
529+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
530+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
531+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
532+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
533+
}
528534

529535
Eigen::Array<double, 7, 1> joint_max_;
530536
Eigen::Array<double, 7, 1> joint_min_;
@@ -613,12 +619,15 @@ void Franka::zero_torque_guiding() {
613619
}
614620

615621
void Franka::zero_torque_controller() {
616-
// high collision threshold values for high impedance
617-
robot.setCollisionBehavior(
618-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
619-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
620-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
621-
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
622+
this->set_default_robot_behavior();
623+
if (this->m_cfg.allow_high_collision) {
624+
// High collision threshold values for high impedance.
625+
robot.setCollisionBehavior(
626+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
627+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
628+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
629+
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
630+
}
622631

623632
this->controller_time = 0.0;
624633
try {

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,7 @@ struct FrankaConfig : common::RobotConfig {
4444
common::Vector7d kd =
4545
(common::Vector7d() << 20., 20., 20., 20., 7.5, 15.0, 5.0).finished();
4646
common::Vector7d torque_limit = common::Vector7d::Constant(5.0);
47+
bool allow_high_collision = false;
4748
// values from deoxys/config/osc-position-controller.yml
4849
Eigen::Vector3d kp_p = (Eigen::Vector3d() << 150., 150., 150.).finished();
4950
double kp_r = 250.0;

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

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -131,6 +131,8 @@ PYBIND11_MODULE(_core, m) {
131131
.def_readwrite("kp", &rcs::hw::FrankaConfig::kp)
132132
.def_readwrite("kd", &rcs::hw::FrankaConfig::kd)
133133
.def_readwrite("torque_limit", &rcs::hw::FrankaConfig::torque_limit)
134+
.def_readwrite("allow_high_collision",
135+
&rcs::hw::FrankaConfig::allow_high_collision)
134136
.def_readwrite("kp_p", &rcs::hw::FrankaConfig::kp_p)
135137
.def_readwrite("kp_r", &rcs::hw::FrankaConfig::kp_r)
136138
.def_readwrite("load_parameters", &rcs::hw::FrankaConfig::load_parameters)
@@ -156,13 +158,15 @@ PYBIND11_MODULE(_core, m) {
156158
bool async_control, bool tcp_offset_configured_in_desk,
157159
bool ignore_realtime, rcs::common::Pose tcp_offset,
158160
std::string attachment_site, std::string kinematic_model_path,
159-
const rcs::common::Vector7d& torque_limit) {
161+
const rcs::common::Vector7d& torque_limit,
162+
bool allow_high_collision) {
160163
rcs::hw::FR3Config cfg;
161164
cfg.ik_solver = ik_solver;
162165
cfg.speed_factor = speed_factor;
163166
cfg.kp = kp;
164167
cfg.kd = kd;
165168
cfg.torque_limit = torque_limit;
169+
cfg.allow_high_collision = allow_high_collision;
166170
cfg.kp_p = kp_p;
167171
cfg.kp_r = kp_r;
168172
cfg.load_parameters = load_parameters;
@@ -196,7 +200,9 @@ PYBIND11_MODULE(_core, m) {
196200
py::arg("attachment_site") = default_fr3_config.attachment_site,
197201
py::arg("kinematic_model_path") =
198202
default_fr3_config.kinematic_model_path,
199-
py::arg("torque_limit") = default_fr3_config.torque_limit);
203+
py::arg("torque_limit") = default_fr3_config.torque_limit,
204+
py::arg("allow_high_collision") =
205+
default_fr3_config.allow_high_collision);
200206
rcs::hw::PandaConfig default_panda_config;
201207
py::class_<rcs::hw::PandaConfig, rcs::hw::FrankaConfig>(hw, "PandaConfig")
202208
.def(py::init(
@@ -210,13 +216,15 @@ PYBIND11_MODULE(_core, m) {
210216
bool async_control, bool tcp_offset_configured_in_desk,
211217
bool ignore_realtime, rcs::common::Pose tcp_offset,
212218
std::string attachment_site, std::string kinematic_model_path,
213-
const rcs::common::Vector7d& torque_limit) {
219+
const rcs::common::Vector7d& torque_limit,
220+
bool allow_high_collision) {
214221
rcs::hw::PandaConfig cfg;
215222
cfg.ik_solver = ik_solver;
216223
cfg.speed_factor = speed_factor;
217224
cfg.kp = kp;
218225
cfg.kd = kd;
219226
cfg.torque_limit = torque_limit;
227+
cfg.allow_high_collision = allow_high_collision;
220228
cfg.kp_p = kp_p;
221229
cfg.kp_r = kp_r;
222230
cfg.load_parameters = load_parameters;
@@ -250,7 +258,9 @@ PYBIND11_MODULE(_core, m) {
250258
py::arg("attachment_site") = default_panda_config.attachment_site,
251259
py::arg("kinematic_model_path") =
252260
default_panda_config.kinematic_model_path,
253-
py::arg("torque_limit") = default_panda_config.torque_limit);
261+
py::arg("torque_limit") = default_panda_config.torque_limit,
262+
py::arg("allow_high_collision") =
263+
default_panda_config.allow_high_collision);
254264

255265
py::object gripper_config =
256266
(py::object)py::module_::import("rcs").attr("common").attr(

0 commit comments

Comments
 (0)