File size: 4,260 Bytes
985c397 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 | // SPDX-License-Identifier: LGPL-2.1-or-later
// Copyright (C) 2009 Dominick Vanthienen <dominick dot vanthienen at mech dot kuleuven dot be>
// Version: 1.0
// Author: Dominick Vanthienen <dominick dot vanthienen at mech dot kuleuven dot be>
// Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
// URL: http://www.orocos.org/kdl
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
#include "chaindynparam.hpp"
#include "frames_io.hpp"
#include <iostream>
namespace KDL {
ChainDynParam::ChainDynParam(const Chain& _chain, Vector _grav):
chain(_chain),
//nr(0),
nj(chain.getNrOfJoints()),
ns(chain.getNrOfSegments()),
grav(_grav),
jntarraynull(nj),
chainidsolver_coriolis( chain, Vector::Zero()),
chainidsolver_gravity( chain, grav),
wrenchnull(ns,Wrench::Zero()),
X(ns),
S(ns),
Ic(ns)
{
ag=-Twist(grav,Vector::Zero());
}
//calculate inertia matrix H
int ChainDynParam::JntToMass(const JntArray &q, JntSpaceInertiaMatrix& H)
{
//Check sizes when in debug mode
if(q.rows()!=nj || H.rows()!=nj || H.columns()!=nj )
return -1;
unsigned int k=0;
double q_;
//Sweep from root to leaf
for(unsigned int i=0;i<ns;i++)
{
//Collect RigidBodyInertia
Ic[i]=chain.getSegment(i).getInertia();
if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{
q_=q(k);
k++;
}
else
{
q_=0.0;
}
X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the frame for transformations from the parent to the current coord frame
S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
}
//Sweep from leaf to root
int j,l;
k=nj-1; //reset k
for(int i=ns-1;i>=0;i--)
{
if(i!=0)
{
//assumption that previous segment is parent
Ic[i-1]=Ic[i-1]+X[i]*Ic[i];
}
F=Ic[i]*S[i];
if(chain.getSegment(i).getJoint().getType()!=Joint::None)
{
H(k,k)=dot(S[i],F);
j=k; //countervariable for the joints
l=i; //countervariable for the segments
while(l!=0) //go from leaf to root starting at i
{
//assumption that previous segment is parent
F=X[l]*F; //calculate the unit force (cfr S) for every segment: F[l-1]=X[l]*F[l]
l--; //go down a segment
if(chain.getSegment(l).getJoint().getType()!=Joint::None) //if the joint connected to segment is not a fixed joint
{
j--;
H(k,j)=dot(F,S[l]); //here you actually match a certain not fixed joint with a segment
H(j,k)=H(k,j);
}
}
k--; //this if-loop should be repeated nj times (k=nj-1 to k=0)
}
}
return 0;
}
//calculate coriolis matrix C
int ChainDynParam::JntToCoriolis(const JntArray &q, const JntArray &q_dot, JntArray &coriolis)
{
//make a null matrix with the size of q_dotdot and a null wrench
SetToZero(jntarraynull);
//the calculation of coriolis matrix C
chainidsolver_coriolis.CartToJnt(q, q_dot, jntarraynull, wrenchnull, coriolis);
return 0;
}
//calculate gravity matrix G
int ChainDynParam::JntToGravity(const JntArray &q,JntArray &gravity)
{
//make a null matrix with the size of q_dotdot and a null wrench
SetToZero(jntarraynull);
//the calculation of coriolis matrix C
chainidsolver_gravity.CartToJnt(q, jntarraynull, jntarraynull, wrenchnull, gravity);
return 0;
}
ChainDynParam::~ChainDynParam()
{
}
}
|