Go to the documentation of this file.00001
00002 #include <kdl/frames_io.hpp>
00003
00004 #include "robodyn_controllers/KdlChainIdInertia.h"
00005
00006 using namespace KDL;
00007
00008
00014 KdlChainIdInertia::KdlChainIdInertia(const Chain& chain_, Vector grav):
00015 chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()),
00016 X(ns), S(ns), v(ns), a(ns), f(ns), Ii(ns)
00017 {
00018 ag = -Twist(grav, Vector::Zero());
00019 }
00020
00031 int KdlChainIdInertia::CartToJnt(const JntArray& q, const JntArray& q_dot, const JntArray& q_dotdot, const Wrenches& forceExt, JntArray& torques, JntArray& Hv)
00032 {
00033
00034 if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || forceExt.size() != ns || Hv.rows() != nj )
00035 {
00036 return -1;
00037 }
00038
00039 unsigned int j = 0;
00040
00041
00042 for (unsigned int i = 0; i < ns; i++)
00043 {
00044 double q_, qdot_, qdotdot_;
00045
00046 if (chain.getSegment(i).getJoint().getType() != Joint::None)
00047 {
00048 q_ = q(j);
00049 qdot_ = q_dot(j);
00050 qdotdot_ = q_dotdot(j);
00051 j++;
00052 }
00053 else
00054 {
00055 q_ = qdot_ = qdotdot_ = 0.0;
00056 }
00057
00058
00059 X[i] = chain.getSegment(i).pose(q_);
00060
00061
00062
00063 Twist vj = X[i].M.Inverse(chain.getSegment(i).twist(q_, qdot_));
00064 S[i] = X[i].M.Inverse(chain.getSegment(i).twist(q_, 1.0));
00065
00066
00067
00068 if (i == 0)
00069 {
00070 v[i] = vj;
00071 a[i] = X[i].Inverse(ag) + S[i] * qdotdot_ + v[i] * vj;
00072 }
00073 else
00074 {
00075 v[i] = X[i].Inverse(v[i - 1]) + vj;
00076 a[i] = X[i].Inverse(a[i - 1]) + S[i] * qdotdot_ + v[i] * vj;
00077 }
00078
00079
00080
00081 Ii[i] = chain.getSegment(i).getInertia();
00082 f[i] = Ii[i] * a[i] + v[i] * (Ii[i] * v[i]) - forceExt[i];
00083 }
00084
00085
00086 j = nj - 1;
00087
00088 for (int i = ns - 1; i >= 0; i--)
00089 {
00090 if (i != 0)
00091 {
00092 f[i - 1] = f[i - 1] + X[i] * f[i];
00093 Ii[i - 1] = Ii[i - 1] + X[i] * Ii[i];
00094 }
00095
00096 Fis = Ii[i] * S[i];
00097
00098 if (chain.getSegment(i).getJoint().getType() != Joint::None)
00099 {
00100 torques(j) = dot(S[i], f[i]);
00101 Hv(j--) = dot(S[i], Fis);
00102 }
00103 }
00104
00105 return 0;
00106 }