Skip to content

Commit a0040ed

Browse files
PeterBowmanPeterBowman
committed
Make invDyn signature refer to tip segment only
#162
1 parent 7645d82 commit a0040ed

File tree

9 files changed

+41
-30
lines changed

9 files changed

+41
-30
lines changed

‎libraries/YarpPlugins/AsibotSolver/AsibotSolver.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -66,7 +66,7 @@ class AsibotSolver : public yarp::dev::DeviceDriver,
6666
bool invDyn(const std::vector<double> &q, std::vector<double> &t) override;
6767

6868
// Perform inverse dynamics.
69-
bool invDyn(const std::vector<double> &q,const std::vector<double> &qdot,const std::vector<double> &qdotdot, const std::vector< std::vector<double> > &fexts, std::vector<double> &t) override;
69+
bool invDyn(const std::vector<double> &q, const std::vector<double> &qdot, const std::vector<double> &qdotdot, const std::vector<double> &ftip, std::vector<double> &t) override;
7070

7171
// -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
7272
bool open(yarp::os::Searchable& config) override;

‎libraries/YarpPlugins/AsibotSolver/ICartesianSolverImpl.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -493,7 +493,7 @@ bool AsibotSolver::invDyn(const std::vector<double> &q,std::vector<double> &t)
493493

494494
// -----------------------------------------------------------------------------
495495

496-
bool AsibotSolver::invDyn(const std::vector<double> &q,const std::vector<double> &qdot,const std::vector<double> &qdotdot, const std::vector< std::vector<double> > &fexts, std::vector<double> &t)
496+
bool AsibotSolver::invDyn(const std::vector<double> &q,const std::vector<double> &qdot,const std::vector<double> &qdotdot, const std::vector<double> &ftip, std::vector<double> &t)
497497
{
498498
yCWarning(ASIBOT) << "invDyn() not implemented";
499499
return false;

‎libraries/YarpPlugins/BasicCartesianControl/PeriodicThreadImpl.cpp

+1-10
Original file line numberDiff line numberDiff line change
@@ -338,18 +338,9 @@ void BasicCartesianControl::handleForc(const std::vector<double> &q, const State
338338
}
339339

340340
std::vector<double> qdot(numJoints), qdotdot(numJoints);
341-
std::vector< std::vector<double> > fexts;
342-
343-
for (int i = 0; i < numJoints - 1; i++) //-- "numJoints - 1" is important
344-
{
345-
fexts.emplace_back(6, 0);
346-
}
347-
348-
fexts.push_back(td);
349-
350341
std::vector<double> t(numJoints);
351342

352-
if (!iCartesianSolver->invDyn(q, qdot, qdotdot, fexts, t))
343+
if (!iCartesianSolver->invDyn(q, qdot, qdotdot, td, t))
353344
{
354345
yCWarning(BCC) << "invDyn() failed";
355346
return;

‎libraries/YarpPlugins/ICartesianSolver.h

+26-2
Original file line numberDiff line numberDiff line change
@@ -159,6 +159,7 @@ class ICartesianSolver
159159
*/
160160
virtual bool invDyn(const std::vector<double> &q, std::vector<double> &t) = 0;
161161

162+
#ifndef SWIG_PREPROCESSOR_SHOULD_SKIP_THIS
162163
/**
163164
* @brief Perform inverse dynamics
164165
*
@@ -174,8 +175,31 @@ class ICartesianSolver
174175
*
175176
* @return true on success, false otherwise
176177
*/
177-
virtual bool invDyn(const std::vector<double> &q,const std::vector<double> &qdot, const std::vector<double> &qdotdot,
178-
const std::vector< std::vector<double> > &fexts, std::vector<double> &t) = 0;
178+
[[deprecated("use `const std::vector<double> &ftip` signature instead")]]
179+
virtual bool invDyn(const std::vector<double> &q, const std::vector<double> &qdot, const std::vector<double> &qdotdot,
180+
const std::vector<std::vector<double>> &fexts, std::vector<double> &t)
181+
{
182+
return invDyn(q, qdot, qdotdot, fexts.back(), t);
183+
}
184+
#endif
185+
186+
/**
187+
* @brief Perform inverse dynamics
188+
*
189+
* @param q Vector describing current position in joint space (meters or degrees).
190+
* @param qdot Vector describing current velocity in joint space (meters/second or degrees/second).
191+
* @param qdotdot Vector describing current acceleration in joint space (meters/second² or degrees/second²).
192+
* @param ftip Vector describing an external force applied to the robot tip, expressed in cartesian space;
193+
* first three elements denote translational acceleration (meters/second²), last three denote
194+
* angular acceleration (radians/second²).
195+
* @param t 6-element vector describing desired forces in cartesian space; first
196+
* three elements denote translational acceleration (meters/second²), last three denote
197+
* angular acceleration (radians/second²).
198+
*
199+
* @return true on success, false otherwise
200+
*/
201+
virtual bool invDyn(const std::vector<double> &q, const std::vector<double> &qdot, const std::vector<double> &qdotdot,
202+
const std::vector<double> &ftip, std::vector<double> &t) = 0;
179203
};
180204

181205
} // namespace roboticslab

‎libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp

+7-9
Original file line numberDiff line numberDiff line change
@@ -222,7 +222,7 @@ bool KdlSolver::diffInvKin(const std::vector<double> &q, const std::vector<doubl
222222

223223
// -----------------------------------------------------------------------------
224224

225-
bool KdlSolver::invDyn(const std::vector<double> &q,std::vector<double> &t)
225+
bool KdlSolver::invDyn(const std::vector<double> &q, std::vector<double> &t)
226226
{
227227
KDL::JntArray qInRad(chain.getNrOfJoints());
228228

@@ -265,7 +265,7 @@ bool KdlSolver::invDyn(const std::vector<double> &q,std::vector<double> &t)
265265

266266
// -----------------------------------------------------------------------------
267267

268-
bool KdlSolver::invDyn(const std::vector<double> &q,const std::vector<double> &qdot,const std::vector<double> &qdotdot, const std::vector< std::vector<double> > &fexts, std::vector<double> &t)
268+
bool KdlSolver::invDyn(const std::vector<double> &q, const std::vector<double> &qdot, const std::vector<double> &qdotdot, const std::vector<double> &ftip, std::vector<double> &t)
269269
{
270270
KDL::JntArray qInRad(chain.getNrOfJoints());
271271

@@ -290,13 +290,11 @@ bool KdlSolver::invDyn(const std::vector<double> &q,const std::vector<double> &q
290290

291291
KDL::Wrenches wrenches(chain.getNrOfSegments(), KDL::Wrench::Zero());
292292

293-
for (int i = 0; i < fexts.size(); i++)
294-
{
295-
wrenches[i] = KDL::Wrench(
296-
KDL::Vector(fexts[i][0], fexts[i][1], fexts[i][2]),
297-
KDL::Vector(fexts[i][3], fexts[i][4], fexts[i][5])
298-
);
299-
}
293+
// FIXME: review this per https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/162
294+
wrenches[chain.getNrOfJoints() - 1] = KDL::Wrench(
295+
KDL::Vector(ftip[0], ftip[1], ftip[2]),
296+
KDL::Vector(ftip[3], ftip[4], ftip[5])
297+
);
300298

301299
KDL::JntArray kdlt(chain.getNrOfJoints());
302300
int ret;

‎libraries/YarpPlugins/KdlSolver/KdlSolver.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ class KdlSolver : public yarp::dev::DeviceDriver,
6868
bool invDyn(const std::vector<double> &q, std::vector<double> &t) override;
6969

7070
// Perform inverse dynamics.
71-
bool invDyn(const std::vector<double> &q,const std::vector<double> &qdot,const std::vector<double> &qdotdot, const std::vector< std::vector<double> > &fexts, std::vector<double> &t) override;
71+
bool invDyn(const std::vector<double> &q, const std::vector<double> &qdot, const std::vector<double> &qdotdot, const std::vector<double> &ftip, std::vector<double> &t) override;
7272

7373
// -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp --------
7474

‎libraries/YarpPlugins/KdlTreeSolver/ICartesianSolverImpl.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -295,7 +295,7 @@ bool KdlTreeSolver::invDyn(const std::vector<double> & q, std::vector<double> &
295295

296296
// -----------------------------------------------------------------------------
297297

298-
bool KdlTreeSolver::invDyn(const std::vector<double> & q, const std::vector<double> & qdot, const std::vector<double> & qdotdot, const std::vector<std::vector<double>> & fexts, std::vector<double> & t)
298+
bool KdlTreeSolver::invDyn(const std::vector<double> & q, const std::vector<double> & qdot, const std::vector<double> & qdotdot, const std::vector<double> & ftip, std::vector<double> & t)
299299
{
300300
KDL::JntArray qInRad(tree.getNrOfJoints());
301301

‎libraries/YarpPlugins/KdlTreeSolver/KdlTreeSolver.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ class KdlTreeSolver : public yarp::dev::DeviceDriver,
7373
bool invDyn(const std::vector<double> & q, std::vector<double> & t) override;
7474

7575
// Perform inverse dynamics.
76-
bool invDyn(const std::vector<double> & q, const std::vector<double> & qdot, const std::vector<double> & qdotdot, const std::vector<std::vector<double>> & fexts, std::vector<double> & t) override;
76+
bool invDyn(const std::vector<double> & q, const std::vector<double> & qdot, const std::vector<double> & qdotdot, const std::vector<double> & ftip, std::vector<double> & t) override;
7777

7878
// -------- DeviceDriver declarations. Implementation in IDeviceImpl.cpp --------
7979

‎tests/testKdlSolver.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -118,11 +118,9 @@ TEST_F(KdlSolverTest, KdlSolverInvDyn2)
118118

119119
TEST_F(KdlSolverTest, KdlSolverInvDyn3)
120120
{
121-
std::vector<double> q(1),qdot(1,0.0),qdotdot(1,0.0),fext(6,0.0),t;
121+
std::vector<double> q(1),qdot(1,0.0),qdotdot(1,0.0),ftip(6,0.0),t;
122122
q[0] = 0.0;
123-
std::vector< std::vector<double> > fexts;
124-
fexts.push_back(fext);
125-
iCartesianSolver->invDyn(q,qdot,qdotdot,fexts,t);
123+
iCartesianSolver->invDyn(q,qdot,qdotdot,ftip,t);
126124
ASSERT_EQ(t.size(), 1 );
127125
ASSERT_NEAR(t[0], 5, 1e-9); //-- T = F*d = 1kg * 10m/s^2 * 0.5m = 5 N*m
128126
}

0 commit comments

Comments
 (0)