Skip to content

Commit 3cb449c

Browse files
juelgjuelg
committed
feat(interface): add get_cartesian_flange_position
1 parent c01979c commit 3cb449c

14 files changed

Lines changed: 160 additions & 132 deletions

File tree

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

Lines changed: 10 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -25,11 +25,8 @@ common::Pose GetFlangeInBaseFrame(const franka::RobotState& robot_state) {
2525
}
2626

2727
common::Pose GetTCPInBaseFrame(const franka::RobotState& robot_state,
28-
const std::optional<common::Pose>& tcp_offset) {
29-
if (!tcp_offset.has_value()) {
30-
return common::Pose(robot_state.O_T_EE);
31-
}
32-
return GetFlangeInBaseFrame(robot_state) * tcp_offset.value();
28+
const common::Pose& tcp_offset) {
29+
return GetFlangeInBaseFrame(robot_state) * tcp_offset;
3330
}
3431

3532
Franka::Franka(const FrankaConfig& cfg,
@@ -406,15 +403,9 @@ void Franka::osc() {
406403
Eigen::Map<const Eigen::Matrix<double, 7, 1>> gravity(
407404
gravity_array.data());
408405

409-
std::array<double, 42> jacobian_array;
410-
if (this->m_cfg.tcp_offset.has_value()) {
411-
jacobian_array = model.zeroJacobian(
412-
franka::Frame::kEndEffector, robot_state.q,
413-
this->m_cfg.tcp_offset->affine_array(), robot_state.EE_T_K);
414-
} else {
415-
jacobian_array =
416-
model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
417-
}
406+
std::array<double, 42> jacobian_array = model.zeroJacobian(
407+
franka::Frame::kEndEffector, robot_state.q,
408+
this->m_cfg.tcp_offset.affine_array(), robot_state.EE_T_K);
418409
Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(
419410
jacobian_array.data());
420411

@@ -769,16 +760,13 @@ void Franka::set_cartesian_position(const common::Pose& x) {
769760
}
770761
if (this->m_cfg.ik_solver == IKSolver::franka_ik) {
771762
const franka::RobotState robot_state = this->robot.readOnce();
772-
common::Pose target_pose = x;
773-
if (this->m_cfg.tcp_offset.has_value()) {
774-
target_pose = x * this->m_cfg.tcp_offset->inverse() *
775-
common::Pose(robot_state.F_T_EE);
776-
}
763+
const common::Pose target_pose =
764+
x * this->m_cfg.tcp_offset.inverse() * common::Pose(robot_state.F_T_EE);
777765
this->set_cartesian_position_internal(target_pose, 1.0, std::nullopt,
778766
std::nullopt);
779767

780768
} else if (this->m_cfg.ik_solver == IKSolver::rcs_ik) {
781-
this->set_cartesian_position_ik(target_pose);
769+
this->set_cartesian_position_ik(x);
782770
}
783771
}
784772

@@ -788,11 +776,8 @@ void Franka::set_cartesian_position_ik(const common::Pose& pose) {
788776
"No inverse kinematics was provided. Cannot use IK to set cartesian "
789777
"position.");
790778
}
791-
const franka::RobotState robot_state = this->robot.readOnce();
792-
const common::Pose tcp_offset =
793-
this->m_cfg.tcp_offset.value_or(common::Pose(robot_state.F_T_EE));
794-
auto joints =
795-
this->m_ik.value()->inverse(pose, this->get_joint_position(), tcp_offset);
779+
auto joints = this->m_ik.value()->inverse(pose, this->get_joint_position(),
780+
this->m_cfg.tcp_offset);
796781

797782
if (joints.has_value()) {
798783
this->set_joint_position(joints.value());

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

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,9 @@ struct FrankaConfig : common::RobotConfig {
5050
double kp_r = 250.0;
5151
std::optional<FrankaLoad> load_parameters = std::nullopt;
5252
std::optional<common::Pose> world_to_robot = std::nullopt;
53-
std::optional<common::Pose> tcp_offset = std::nullopt;
53+
common::Pose tcp_offset = common::Pose::Identity();
54+
// Indicates that Cartesian control uses tcp_offset.
55+
bool tcp_offset_explicit = false;
5456
bool async_control = false;
5557
bool ignore_realtime = false;
5658
size_t dof = 7;
@@ -119,7 +121,7 @@ class Franka : public common::Robot {
119121

120122
common::Pose get_cartesian_position() override;
121123

122-
common::Pose get_cartesian_flange_position();
124+
common::Pose get_cartesian_flange_position() override;
123125

124126
void set_joint_position(const common::VectorXd& q) override;
125127

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

Lines changed: 108 additions & 97 deletions
Original file line numberDiff line numberDiff line change
@@ -137,111 +137,122 @@ PYBIND11_MODULE(_core, m) {
137137
.def_readwrite("kp_r", &rcs::hw::FrankaConfig::kp_r)
138138
.def_readwrite("load_parameters", &rcs::hw::FrankaConfig::load_parameters)
139139
.def_readwrite("world_to_robot", &rcs::hw::FrankaConfig::world_to_robot)
140-
.def_readwrite("tcp_offset", &rcs::hw::FrankaConfig::tcp_offset)
140+
.def_property(
141+
"tcp_offset",
142+
[](const rcs::hw::FrankaConfig& config) { return config.tcp_offset; },
143+
[](rcs::hw::FrankaConfig& config,
144+
const rcs::common::Pose& tcp_offset) {
145+
config.tcp_offset = tcp_offset;
146+
config.tcp_offset_explicit = true;
147+
})
148+
.def_readwrite("tcp_offset_explicit",
149+
&rcs::hw::FrankaConfig::tcp_offset_explicit)
141150
.def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control)
142151
.def_readwrite("ignore_realtime", &rcs::hw::FrankaConfig::ignore_realtime)
143152
.def_readwrite("ip", &rcs::hw::FrankaConfig::ip);
144153

145154
rcs::hw::FR3Config default_fr3_config;
146155
py::class_<rcs::hw::FR3Config, rcs::hw::FrankaConfig>(hw, "FR3Config")
147-
.def(py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver,
148-
double speed_factor, const rcs::common::Vector7d& kp,
149-
const rcs::common::Vector7d& kd,
150-
const Eigen::Vector3d& kp_p, double kp_r,
151-
std::optional<rcs::hw::FrankaLoad> load_parameters,
152-
std::optional<rcs::common::Pose> world_to_robot,
153-
bool async_control, bool ignore_realtime,
154-
std::optional<rcs::common::Pose> tcp_offset,
155-
std::string attachment_site,
156-
std::string kinematic_model_path,
157-
const rcs::common::Vector7d& torque_limit,
158-
bool allow_high_collision) {
159-
rcs::hw::FR3Config cfg;
160-
cfg.ik_solver = ik_solver;
161-
cfg.speed_factor = speed_factor;
162-
cfg.kp = kp;
163-
cfg.kd = kd;
164-
cfg.torque_limit = torque_limit;
165-
cfg.allow_high_collision = allow_high_collision;
166-
cfg.kp_p = kp_p;
167-
cfg.kp_r = kp_r;
168-
cfg.load_parameters = load_parameters;
169-
cfg.world_to_robot = world_to_robot;
170-
cfg.async_control = async_control;
171-
cfg.ignore_realtime = ignore_realtime;
172-
cfg.ip = ip;
173-
cfg.tcp_offset = tcp_offset;
174-
cfg.attachment_site = attachment_site;
175-
cfg.kinematic_model_path = kinematic_model_path;
176-
return cfg;
177-
}),
178-
py::arg("ip"), py::arg("ik_solver") = default_fr3_config.ik_solver,
179-
py::arg("speed_factor") = default_fr3_config.speed_factor,
180-
py::arg("kp") = default_fr3_config.kp,
181-
py::arg("kd") = default_fr3_config.kd,
182-
py::arg("kp_p") = default_fr3_config.kp_p,
183-
py::arg("kp_r") = default_fr3_config.kp_r,
184-
py::arg("load_parameters") = default_fr3_config.load_parameters,
185-
py::arg("world_to_robot") = default_fr3_config.world_to_robot,
186-
py::arg("async_control") = default_fr3_config.async_control,
187-
py::arg("ignore_realtime") = default_fr3_config.ignore_realtime,
188-
py::arg("tcp_offset") = default_fr3_config.tcp_offset,
189-
py::arg("attachment_site") = default_fr3_config.attachment_site,
190-
py::arg("kinematic_model_path") =
191-
default_fr3_config.kinematic_model_path,
192-
py::arg("torque_limit") = default_fr3_config.torque_limit,
193-
py::arg("allow_high_collision") =
194-
default_fr3_config.allow_high_collision);
156+
.def(
157+
py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver,
158+
double speed_factor, const rcs::common::Vector7d& kp,
159+
const rcs::common::Vector7d& kd,
160+
const Eigen::Vector3d& kp_p, double kp_r,
161+
std::optional<rcs::hw::FrankaLoad> load_parameters,
162+
std::optional<rcs::common::Pose> world_to_robot,
163+
bool async_control, bool ignore_realtime,
164+
rcs::common::Pose tcp_offset, std::string attachment_site,
165+
std::string kinematic_model_path,
166+
const rcs::common::Vector7d& torque_limit,
167+
bool allow_high_collision) {
168+
rcs::hw::FR3Config cfg;
169+
cfg.ik_solver = ik_solver;
170+
cfg.speed_factor = speed_factor;
171+
cfg.kp = kp;
172+
cfg.kd = kd;
173+
cfg.torque_limit = torque_limit;
174+
cfg.allow_high_collision = allow_high_collision;
175+
cfg.kp_p = kp_p;
176+
cfg.kp_r = kp_r;
177+
cfg.load_parameters = load_parameters;
178+
cfg.world_to_robot = world_to_robot;
179+
cfg.async_control = async_control;
180+
cfg.ignore_realtime = ignore_realtime;
181+
cfg.ip = ip;
182+
cfg.tcp_offset = tcp_offset;
183+
cfg.tcp_offset_explicit = true;
184+
cfg.attachment_site = attachment_site;
185+
cfg.kinematic_model_path = kinematic_model_path;
186+
return cfg;
187+
}),
188+
py::arg("ip"), py::arg("ik_solver") = default_fr3_config.ik_solver,
189+
py::arg("speed_factor") = default_fr3_config.speed_factor,
190+
py::arg("kp") = default_fr3_config.kp,
191+
py::arg("kd") = default_fr3_config.kd,
192+
py::arg("kp_p") = default_fr3_config.kp_p,
193+
py::arg("kp_r") = default_fr3_config.kp_r,
194+
py::arg("load_parameters") = default_fr3_config.load_parameters,
195+
py::arg("world_to_robot") = default_fr3_config.world_to_robot,
196+
py::arg("async_control") = default_fr3_config.async_control,
197+
py::arg("ignore_realtime") = default_fr3_config.ignore_realtime,
198+
py::arg("tcp_offset") = default_fr3_config.tcp_offset,
199+
py::arg("attachment_site") = default_fr3_config.attachment_site,
200+
py::arg("kinematic_model_path") =
201+
default_fr3_config.kinematic_model_path,
202+
py::arg("torque_limit") = default_fr3_config.torque_limit,
203+
py::arg("allow_high_collision") =
204+
default_fr3_config.allow_high_collision);
195205
rcs::hw::PandaConfig default_panda_config;
196206
py::class_<rcs::hw::PandaConfig, rcs::hw::FrankaConfig>(hw, "PandaConfig")
197-
.def(py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver,
198-
double speed_factor, const rcs::common::Vector7d& kp,
199-
const rcs::common::Vector7d& kd,
200-
const Eigen::Vector3d& kp_p, double kp_r,
201-
std::optional<rcs::hw::FrankaLoad> load_parameters,
202-
std::optional<rcs::common::Pose> world_to_robot,
203-
bool async_control, bool ignore_realtime,
204-
std::optional<rcs::common::Pose> tcp_offset,
205-
std::string attachment_site,
206-
std::string kinematic_model_path,
207-
const rcs::common::Vector7d& torque_limit,
208-
bool allow_high_collision) {
209-
rcs::hw::PandaConfig cfg;
210-
cfg.ik_solver = ik_solver;
211-
cfg.speed_factor = speed_factor;
212-
cfg.kp = kp;
213-
cfg.kd = kd;
214-
cfg.torque_limit = torque_limit;
215-
cfg.allow_high_collision = allow_high_collision;
216-
cfg.kp_p = kp_p;
217-
cfg.kp_r = kp_r;
218-
cfg.load_parameters = load_parameters;
219-
cfg.world_to_robot = world_to_robot;
220-
cfg.async_control = async_control;
221-
cfg.ignore_realtime = ignore_realtime;
222-
cfg.ip = ip;
223-
cfg.tcp_offset = tcp_offset;
224-
cfg.attachment_site = attachment_site;
225-
cfg.kinematic_model_path = kinematic_model_path;
226-
return cfg;
227-
}),
228-
py::arg("ip"), py::arg("ik_solver") = default_panda_config.ik_solver,
229-
py::arg("speed_factor") = default_panda_config.speed_factor,
230-
py::arg("kp") = default_panda_config.kp,
231-
py::arg("kd") = default_panda_config.kd,
232-
py::arg("kp_p") = default_panda_config.kp_p,
233-
py::arg("kp_r") = default_panda_config.kp_r,
234-
py::arg("load_parameters") = default_panda_config.load_parameters,
235-
py::arg("world_to_robot") = default_panda_config.world_to_robot,
236-
py::arg("async_control") = default_panda_config.async_control,
237-
py::arg("ignore_realtime") = default_panda_config.ignore_realtime,
238-
py::arg("tcp_offset") = default_panda_config.tcp_offset,
239-
py::arg("attachment_site") = default_panda_config.attachment_site,
240-
py::arg("kinematic_model_path") =
241-
default_panda_config.kinematic_model_path,
242-
py::arg("torque_limit") = default_panda_config.torque_limit,
243-
py::arg("allow_high_collision") =
244-
default_panda_config.allow_high_collision);
207+
.def(
208+
py::init([](const std::string& ip, rcs::hw::IKSolver ik_solver,
209+
double speed_factor, const rcs::common::Vector7d& kp,
210+
const rcs::common::Vector7d& kd,
211+
const Eigen::Vector3d& kp_p, double kp_r,
212+
std::optional<rcs::hw::FrankaLoad> load_parameters,
213+
std::optional<rcs::common::Pose> world_to_robot,
214+
bool async_control, bool ignore_realtime,
215+
rcs::common::Pose tcp_offset, std::string attachment_site,
216+
std::string kinematic_model_path,
217+
const rcs::common::Vector7d& torque_limit,
218+
bool allow_high_collision) {
219+
rcs::hw::PandaConfig cfg;
220+
cfg.ik_solver = ik_solver;
221+
cfg.speed_factor = speed_factor;
222+
cfg.kp = kp;
223+
cfg.kd = kd;
224+
cfg.torque_limit = torque_limit;
225+
cfg.allow_high_collision = allow_high_collision;
226+
cfg.kp_p = kp_p;
227+
cfg.kp_r = kp_r;
228+
cfg.load_parameters = load_parameters;
229+
cfg.world_to_robot = world_to_robot;
230+
cfg.async_control = async_control;
231+
cfg.ignore_realtime = ignore_realtime;
232+
cfg.ip = ip;
233+
cfg.tcp_offset = tcp_offset;
234+
cfg.tcp_offset_explicit = true;
235+
cfg.attachment_site = attachment_site;
236+
cfg.kinematic_model_path = kinematic_model_path;
237+
return cfg;
238+
}),
239+
py::arg("ip"), py::arg("ik_solver") = default_panda_config.ik_solver,
240+
py::arg("speed_factor") = default_panda_config.speed_factor,
241+
py::arg("kp") = default_panda_config.kp,
242+
py::arg("kd") = default_panda_config.kd,
243+
py::arg("kp_p") = default_panda_config.kp_p,
244+
py::arg("kp_r") = default_panda_config.kp_r,
245+
py::arg("load_parameters") = default_panda_config.load_parameters,
246+
py::arg("world_to_robot") = default_panda_config.world_to_robot,
247+
py::arg("async_control") = default_panda_config.async_control,
248+
py::arg("ignore_realtime") = default_panda_config.ignore_realtime,
249+
py::arg("tcp_offset") = default_panda_config.tcp_offset,
250+
py::arg("attachment_site") = default_panda_config.attachment_site,
251+
py::arg("kinematic_model_path") =
252+
default_panda_config.kinematic_model_path,
253+
py::arg("torque_limit") = default_panda_config.torque_limit,
254+
py::arg("allow_high_collision") =
255+
default_panda_config.allow_high_collision);
245256

246257
py::object gripper_config =
247258
(py::object)py::module_::import("rcs").attr("common").attr(

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

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -115,7 +115,8 @@ class FrankaConfig(rcs._core.common.RobotConfig):
115115
kp_r: float
116116
load_parameters: FrankaLoad | None
117117
speed_factor: float
118-
tcp_offset: rcs._core.common.Pose | None
118+
tcp_offset: rcs._core.common.Pose
119+
tcp_offset_explicit: bool
119120
torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
120121
world_to_robot: rcs._core.common.Pose | None
121122

@@ -312,7 +313,7 @@ class FR3Config(FrankaConfig):
312313
world_to_robot: rcs._core.common.Pose | None = None,
313314
async_control: bool = False,
314315
ignore_realtime: bool = False,
315-
tcp_offset: rcs._core.common.Pose | None = None,
316+
tcp_offset: rcs._core.common.Pose = ...,
316317
attachment_site: str = "attachment_site",
317318
kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml",
318319
torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
@@ -333,7 +334,7 @@ class PandaConfig(FrankaConfig):
333334
world_to_robot: rcs._core.common.Pose | None = None,
334335
async_control: bool = False,
335336
ignore_realtime: bool = False,
336-
tcp_offset: rcs._core.common.Pose | None = None,
337+
tcp_offset: rcs._core.common.Pose = ...,
337338
attachment_site: str = "attachment_site",
338339
kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml",
339340
torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,

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

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -115,7 +115,8 @@ class FrankaConfig(rcs._core.common.RobotConfig):
115115
kp_r: float
116116
load_parameters: FrankaLoad | None
117117
speed_factor: float
118-
tcp_offset: rcs._core.common.Pose | None
118+
tcp_offset: rcs._core.common.Pose
119+
tcp_offset_explicit: bool
119120
torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]]
120121
world_to_robot: rcs._core.common.Pose | None
121122

@@ -312,7 +313,7 @@ class FR3Config(FrankaConfig):
312313
world_to_robot: rcs._core.common.Pose | None = None,
313314
async_control: bool = False,
314315
ignore_realtime: bool = False,
315-
tcp_offset: rcs._core.common.Pose | None = None,
316+
tcp_offset: rcs._core.common.Pose = ...,
316317
attachment_site: str = "attachment_site",
317318
kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml",
318319
torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,
@@ -333,7 +334,7 @@ class PandaConfig(FrankaConfig):
333334
world_to_robot: rcs._core.common.Pose | None = None,
334335
async_control: bool = False,
335336
ignore_realtime: bool = False,
336-
tcp_offset: rcs._core.common.Pose | None = None,
337+
tcp_offset: rcs._core.common.Pose = ...,
337338
attachment_site: str = "attachment_site",
338339
kinematic_model_path: str = "assets/scenes/fr3_empty_world/robot.xml",
339340
torque_limit: numpy.ndarray[tuple[typing.Literal[7]], numpy.dtype[numpy.float64]] = ...,

‎extensions/rcs_so101/src/rcs_so101/hw.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,9 @@ def __init__(self, cfg: SO101Config, ik: common.Kinematics):
5050
def get_cartesian_position(self) -> common.Pose:
5151
return self.ik.forward(self.get_joint_position())
5252

53+
def get_cartesian_flange_position(self) -> common.Pose:
54+
return self.get_cartesian_position()
55+
5356
def get_ik(self) -> common.Kinematics | None:
5457
return self.ik
5558

‎extensions/rcs_ur5e/src/rcs_ur5e/hw.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -269,6 +269,9 @@ def get_cartesian_position(self) -> common.Pose:
269269
common.Pose(rpy_vector=np.array([0, 0, np.deg2rad(180)]), translation=np.array([0, 0, 0])).inverse() * pose # type: ignore
270270
)
271271

272+
def get_cartesian_flange_position(self) -> common.Pose:
273+
return self.get_cartesian_position()
274+
272275
def get_ik(self) -> common.Kinematics | None:
273276
return self.ik
274277

‎extensions/rcs_xarm7/src/rcs_xarm7/hw.py‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,9 @@ def get_cartesian_position(self) -> common.Pose:
6060

6161
return common.Pose(rpy_vector=rpy, translation=translation_meter) # type: ignore
6262

63+
def get_cartesian_flange_position(self) -> common.Pose:
64+
return self.get_cartesian_position()
65+
6366
def get_ik(self) -> common.Kinematics | None:
6467
return self.ik
6568

0 commit comments

Comments
 (0)