Skip to content

Commit 6d6ec8e

Browse files
chpohlChristoph Pohl
chpohl
and
Christoph Pohl
authored
Add interface methods to get twists and jacobians in WORLD (#6)
* Add interface methods to get twists and jacobians in WORLD * directly return values instead of creating temporary variable --------- Co-authored-by: Christoph Pohl <christoph.pohl@kit.edu>
1 parent b7ccbae commit 6d6ec8e

5 files changed

Lines changed: 76 additions & 2 deletions

File tree

‎include/xbot2_interface/xbotinterface2.h‎

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -210,6 +210,8 @@ class XBOT2IFC_API XBotInterface : public ReadStateInterface<XBotInterface>
210210
// absolute
211211
virtual void getJacobian(int link_id,
212212
MatRef J) const = 0;
213+
virtual void getJacobianInWorld(int link_id,
214+
MatRef J) const = 0;
213215

214216
bool getJacobian(string_const_ref link_name,
215217
MatRef J) const;
@@ -226,6 +228,7 @@ class XBOT2IFC_API XBotInterface : public ReadStateInterface<XBotInterface>
226228
Eigen::MatrixXd& J) const;
227229

228230
Eigen::MatrixXd getJacobian(string_const_ref link_name) const;
231+
Eigen::MatrixXd getJacobianInWorld(string_const_ref link_name) const;
229232

230233
// relative
231234
virtual void getRelativeJacobian(int distal_id,
@@ -271,13 +274,17 @@ class XBOT2IFC_API XBotInterface : public ReadStateInterface<XBotInterface>
271274
Eigen::Affine3d& b_R_l) const;
272275

273276
// velocity twist (absolute)
274-
virtual Eigen::Vector6d getVelocityTwist(int link_id) const;
275-
276277
Eigen::Vector6d getVelocityTwist(string_const_ref link_name) const;
277278

279+
virtual Eigen::Vector6d getVelocityTwist(int link_id) const;
280+
278281
bool getVelocityTwist(string_const_ref link_name,
279282
Eigen::Vector6d& v) const;
280283

284+
// velocity twist (absolute) in global
285+
Eigen::Vector6d getVelocityTwistInWorld(string_const_ref link_name) const;
286+
virtual Eigen::Vector6d getVelocityTwistInWorld(int link_id) const = 0;
287+
281288
// acceleration twist (absolute)
282289
virtual Eigen::Vector6d getAccelerationTwist(int link_id) const;
283290

@@ -286,6 +293,10 @@ class XBOT2IFC_API XBotInterface : public ReadStateInterface<XBotInterface>
286293
bool getAccelerationTwist(string_const_ref link_name,
287294
Eigen::Vector6d& a) const;
288295

296+
// acceleration twist (absolute) in global
297+
Eigen::Vector6d getAccelerationTwistInWorld(string_const_ref link_name) const;
298+
virtual Eigen::Vector6d getAccelerationTwistInWorld(int link_id) const = 0;
299+
289300
//
290301
virtual Eigen::Vector6d getJdotTimesV(int link_id) const;
291302

‎pinocchio/modelinterface2_pin.cpp‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -115,6 +115,21 @@ void ModelInterface2Pin::getJacobian(int link_id, MatRef J) const
115115
pinocchio::getFrameJacobian(_mdl, _data, link_id, _world_aligned, J);
116116
}
117117

118+
void ModelInterface2Pin::getJacobianInWorld(int link_id, MatRef J) const
119+
{
120+
check_frame_idx_throw(link_id);
121+
122+
if(!(_cached_computation & Jacobians))
123+
{
124+
pinocchio::computeJointJacobians(_mdl, _data, getJointPosition());
125+
_cached_computation |= Jacobians;
126+
}
127+
128+
J.setZero();
129+
130+
pinocchio::getFrameJacobian(_mdl, _data, link_id, pinocchio::ReferenceFrame::WORLD, J);
131+
}
132+
118133

119134
MatConstRef ModelInterface2Pin::computeRegressor() const
120135
{
@@ -371,13 +386,27 @@ Eigen::Vector6d XBot::ModelInterface2Pin::getVelocityTwist(int frame_idx) const
371386
return v;
372387
}
373388

389+
Eigen::Vector6d XBot::ModelInterface2Pin::getVelocityTwistInWorld(int frame_idx) const
390+
{
391+
check_frame_idx_throw(frame_idx);
392+
393+
return pinocchio::getFrameVelocity(_mdl, _data, frame_idx, pinocchio::ReferenceFrame::WORLD);
394+
}
395+
374396
Eigen::Vector6d ModelInterface2Pin::getAccelerationTwist(int frame_idx) const
375397
{
376398
check_frame_idx_throw(frame_idx);
377399

378400
return pinocchio::getFrameClassicalAcceleration(_mdl, _data, frame_idx, _world_aligned);
379401
}
380402

403+
Eigen::Vector6d XBot::ModelInterface2Pin::getAccelerationTwistInWorld(int frame_idx) const
404+
{
405+
check_frame_idx_throw(frame_idx);
406+
407+
return pinocchio::getFrameClassicalAcceleration(_mdl, _data, frame_idx, pinocchio::ReferenceFrame::WORLD);
408+
}
409+
381410
Eigen::Vector6d ModelInterface2Pin::getJdotTimesV(int frame_idx) const
382411
{
383412
check_frame_idx_throw(frame_idx);

‎pinocchio/modelinterface2_pin.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,10 +26,13 @@ class ModelInterface2Pin : public ModelInterface
2626
Eigen::Affine3d getPose(int link_id) const override;
2727

2828
void getJacobian(int link_id, MatRef J) const override;
29+
void getJacobianInWorld(int link_id, MatRef J) const override;
2930

3031
Eigen::Vector6d getVelocityTwist(int link_id) const override;
32+
Eigen::Vector6d getVelocityTwistInWorld(int link_id) const override;
3133

3234
Eigen::Vector6d getAccelerationTwist(int link_id) const override;
35+
Eigen::Vector6d getAccelerationTwistInWorld(int link_id) const override;
3336

3437
Eigen::Vector6d getJdotTimesV(int link_id) const override;
3538

‎python/pyxbot2_interface.cpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -153,8 +153,12 @@ PYBIND11_MODULE(pyxbot2_interface, m) {
153153
py::overload_cast<string_const_ref, string_const_ref>(&XBotInterface::getPose, py::const_))
154154
.def("getAccelerationTwist",
155155
py::overload_cast<string_const_ref>(&XBotInterface::getAccelerationTwist, py::const_))
156+
.def("getAccelerationTwistInWorld",
157+
py::overload_cast<string_const_ref>(&XBotInterface::getAccelerationTwistInWorld, py::const_))
156158
.def("getVelocityTwist",
157159
py::overload_cast<string_const_ref>(&XBotInterface::getVelocityTwist, py::const_))
160+
.def("getVelocityTwistInWorld",
161+
py::overload_cast<string_const_ref>(&XBotInterface::getVelocityTwistInWorld, py::const_))
158162
.def("getJdotTimesV",
159163
py::overload_cast<string_const_ref>(&XBotInterface::getJdotTimesV, py::const_))
160164
.def("getCOM",
@@ -169,6 +173,8 @@ PYBIND11_MODULE(pyxbot2_interface, m) {
169173
py::overload_cast<string_const_ref, string_const_ref>(&XBotInterface::getRelativeJdotTimesV, py::const_))
170174
.def("getJacobian",
171175
py::overload_cast<string_const_ref>(&XBotInterface::getJacobian, py::const_))
176+
.def("getJacobianInWorld",
177+
py::overload_cast<string_const_ref>(&XBotInterface::getJacobianInWorld, py::const_))
172178
.def("getRelativeJacobian",
173179
py::overload_cast<string_const_ref, string_const_ref>(&XBotInterface::getRelativeJacobian, py::const_))
174180
.def("computeInverseDynamics",
@@ -277,6 +283,8 @@ PYBIND11_MODULE(pyxbot2_interface, m) {
277283
py::overload_cast<VecConstRef>(&ModelInterface::setJointEffort))
278284
.def("getInverseJacobian",
279285
[](const ModelInterface& self, const std::string& link_id) { return pinv_SVD(self.getJacobian(link_id)); })
286+
.def("getInverseJacobianInWorld",
287+
[](const ModelInterface& self, const std::string& link_id) { return pinv_SVD(self.getJacobianInWorld(link_id)); })
280288
;
281289

282290
py::class_<RobotInterface, XBotInterface, RobotInterface::Ptr>(m, "RobotInterface2")

‎src/xbotinterface2.cpp‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1078,6 +1078,19 @@ Eigen::MatrixXd XBotInterface::getJacobian(string_const_ref link_name) const
10781078
return J;
10791079
}
10801080

1081+
Eigen::MatrixXd XBotInterface::getJacobianInWorld(string_const_ref link_name) const
1082+
{
1083+
Eigen::MatrixXd J;
1084+
1085+
J.setZero(6, getNv());
1086+
1087+
int idx = impl->get_link_id_throw(link_name);
1088+
1089+
getJacobianInWorld(idx, J);
1090+
1091+
return J;
1092+
}
1093+
10811094

10821095
void XBotInterface::getRelativeJacobian(int distal_id, int base_id, MatRef Jrel) const
10831096
{
@@ -1247,6 +1260,11 @@ Eigen::Vector6d XBotInterface::getVelocityTwist(string_const_ref link_name) cons
12471260
return getVelocityTwist(impl->get_link_id_throw(link_name));
12481261
}
12491262

1263+
Eigen::Vector6d XBotInterface::getVelocityTwistInWorld(string_const_ref link_name) const
1264+
{
1265+
return getVelocityTwistInWorld(impl->get_link_id_throw(link_name));
1266+
}
1267+
12501268
bool XBotInterface::getVelocityTwist(string_const_ref link_name, Eigen::Vector6d &v) const
12511269
{
12521270
int idx = impl->get_link_id_error(link_name);
@@ -1274,6 +1292,11 @@ Eigen::Vector6d XBotInterface::getAccelerationTwist(string_const_ref link_name)
12741292
return getAccelerationTwist(impl->get_link_id_throw(link_name));
12751293
}
12761294

1295+
Eigen::Vector6d XBotInterface::getAccelerationTwistInWorld(string_const_ref link_name) const
1296+
{
1297+
return getAccelerationTwistInWorld(impl->get_link_id_throw(link_name));
1298+
}
1299+
12771300
bool XBotInterface::getAccelerationTwist(string_const_ref link_name, Eigen::Vector6d &v) const
12781301
{
12791302
int idx = impl->get_link_id_error(link_name);

0 commit comments

Comments
 (0)