28 using namespace yarp::os;
29 using namespace yarp::sig;
30 using namespace yarp::math;
44 RigidBodyTransformation::RigidBodyTransformation(
iDynLimb *_limb,
const yarp::sig::Matrix &_H,
const string &_info,
bool _hasSensor,
const FlowType kin,
const FlowType wre,
const NewEulMode _mode,
unsigned int verb)
52 F.resize(3);
F.zero();
53 Mu.resize(3); Mu.zero();
54 w.resize(3); w.zero();
55 dw.resize(3); dw.zero();
56 ddp.resize(3); ddp.zero();
57 H.resize(4,4);
H.eye();
61 RigidBodyTransformation::~RigidBodyTransformation()
67 bool RigidBodyTransformation::setRBT(
const Matrix &_H)
69 if((_H.cols()==4)&&(_H.rows()==4))
76 if(verbose)
fprintf(stderr,
"RigidBodyTransformation: could not set RBT due to wrong sized matrix H: %zu,%zu instead of (4,4). Setting identity as default. \n",_H.cols(),_H.rows());
83 bool RigidBodyTransformation::setKinematic(
const Vector &wNode,
const Vector &dwNode,
const Vector &ddpNode)
92 return limb->initKinematicNewtonEuler(w,dw,ddp);
95 bool RigidBodyTransformation::setKinematicMeasure(
const Vector &w0,
const Vector &dw0,
const Vector &ddp0)
97 return limb->initKinematicNewtonEuler(w0,dw0,ddp0);
100 bool RigidBodyTransformation::setWrench(
const Vector &FNode,
const Vector &MuNode)
108 return limb->initWrenchNewtonEuler(
F,Mu);
111 bool RigidBodyTransformation::setWrenchMeasure(
const Vector &F0,
const Vector &Mu0)
113 return limb->initWrenchNewtonEuler(F0,Mu0);
116 bool RigidBodyTransformation::setWrenchMeasure(
iDynSensor *sensor,
const Vector &Fsens,
const Vector &Musens)
121 Matrix RigidBodyTransformation::getRBT()
const
126 Matrix RigidBodyTransformation::getR()
128 return H.submatrix(0,2,0,2);
131 Matrix RigidBodyTransformation::getR6()
const
133 Matrix R(6,6); R.zero();
145 Vector RigidBodyTransformation::getr(
bool proj)
148 return H.submatrix(0,2,0,3).getCol(3);
149 return getR().transposed() *
H.getCol(3).subVector(0,2);
152 void RigidBodyTransformation::getKinematic(Vector &wNode, Vector &dwNode, Vector &ddpNode)
155 limb->getKinematicNewtonEuler(w,dw,ddp);
166 void RigidBodyTransformation::getWrench(Vector &FNode, Vector &MuNode)
169 limb->getWrenchNewtonEuler(
F,Mu);
176 MuNode = MuNode + Mu;
185 FlowType RigidBodyTransformation::getKinematicFlow()
const
190 FlowType RigidBodyTransformation::getWrenchFlow()
const
195 void RigidBodyTransformation::computeKinematic()
209 ddp = getR() * ( ddp -
cross(dw,getr(
true)) -
cross(w,
cross(w,getr(
true))) ) ;
232 w = getR().transposed() * w ;
233 dw = getR().transposed() * dw;
234 ddp = getR().transposed() * (ddp) +
cross(dw,getr(
true)) +
cross(w,
cross(w,getr(
true))) ;
239 ddp = getR().transposed() * ddp ;
246 void RigidBodyTransformation::computeWrench()
256 Mu =
cross( getr(), getR()*
F ) + getR() * Mu ;
267 Mu = getR().transposed() * ( Mu -
cross(getr(),getR() *
F ));
268 F = getR().transposed() *
F ;
273 bool RigidBodyTransformation::isSensorized()
const
278 void RigidBodyTransformation::computeLimbKinematic()
280 limb->computeKinematicNewtonEuler();
283 void RigidBodyTransformation::computeLimbWrench()
285 limb->computeWrenchNewtonEuler();
288 unsigned int RigidBodyTransformation::getNLinks()
const
293 unsigned int RigidBodyTransformation::getDOF()
const
295 return limb->getDOF();
298 Matrix RigidBodyTransformation::getH(
const unsigned int iLink,
const bool allLink)
300 return limb->getH(iLink,allLink);
303 Matrix RigidBodyTransformation::getH()
308 Vector RigidBodyTransformation::getEndEffPose(
const bool axisRep)
310 return limb->EndEffPose(axisRep);
313 Matrix RigidBodyTransformation::computeGeoJacobian(
const unsigned int iLink,
const Matrix &Pn,
bool rbtRoto)
316 return limb->computeGeoJacobian(iLink,Pn);
318 return getR6() * limb->computeGeoJacobian(iLink,Pn);
321 Matrix RigidBodyTransformation::computeGeoJacobian(
const unsigned int iLink,
const Matrix &Pn,
const Matrix &H0,
bool rbtRoto)
324 return limb->computeGeoJacobian(iLink,Pn,H0);
326 return getR6() * limb->computeGeoJacobian(iLink,Pn,H0);
329 Matrix RigidBodyTransformation::computeGeoJacobian(
const Matrix &Pn,
bool rbtRoto)
332 return limb->computeGeoJacobian(Pn);
334 return getR6() * limb->computeGeoJacobian(Pn);
337 Matrix RigidBodyTransformation::computeGeoJacobian(
const Matrix &Pn,
const Matrix &H0,
bool rbtRoto)
340 return limb->computeGeoJacobian(Pn,H0);
342 return getR6() * limb->computeGeoJacobian(Pn,H0);
345 Matrix RigidBodyTransformation::computeGeoJacobian(
bool rbtRoto)
348 return limb->GeoJacobian();
350 return getR6() * limb->GeoJacobian();
353 Matrix RigidBodyTransformation::computeGeoJacobian(
const unsigned int iLink,
bool rbtRoto)
356 return limb->GeoJacobian(iLink);
358 return getR6() * limb->GeoJacobian(iLink);
361 Matrix RigidBodyTransformation::getH0()
const
363 return limb->getH0();
366 bool RigidBodyTransformation::setH0(
const Matrix &_H0)
368 return limb->setH0(_H0);
377 Matrix RigidBodyTransformation::TESTING_computeCOMJacobian(
const unsigned int iLink,
bool rbtRoto)
380 return limb->TESTING_computeCOMJacobian(iLink);
382 return getR6() * limb->TESTING_computeCOMJacobian(iLink);
385 Matrix RigidBodyTransformation::TESTING_computeCOMJacobian(
const unsigned int iLink,
const Matrix &Pn,
bool rbtRoto)
388 return limb->TESTING_computeCOMJacobian(iLink, Pn);
390 return getR6() * limb->TESTING_computeCOMJacobian(iLink, Pn);
393 Matrix RigidBodyTransformation::TESTING_computeCOMJacobian(
const unsigned int iLink,
const Matrix &Pn,
const Matrix &_H0,
bool rbtRoto)
396 return limb->TESTING_computeCOMJacobian(iLink, Pn, _H0);
398 return getR6() * limb->TESTING_computeCOMJacobian(iLink, Pn, _H0);
401 Matrix RigidBodyTransformation::getHCOM(
unsigned int iLink)
403 return limb->getHCOM(iLink);
424 iDynNode::iDynNode(
const string &_info,
const NewEulMode _mode,
unsigned int verb)
435 w.resize(3); w.zero();
436 dw.resize(3); dw.zero();
437 ddp.resize(3); ddp.zero();
438 F.resize(3);
F.zero();
439 Mu.resize(3); Mu.zero();
444 string infoRbt = limb->
getType() +
" to node";
446 rbtList.push_back(rbt);
449 Matrix iDynNode::getRBT(
unsigned int iLimb)
const
451 if(iLimb<rbtList.size())
453 return rbtList[iLimb].getRBT();
457 if(verbose)
fprintf(stderr,
"iDynNode: error, could not getRBT() due to out of range index: %d , while we have %d limbs. \n", iLimb, (
int)rbtList.size() );
462 bool iDynNode::solveKinematics()
464 unsigned int inputNode=0;
468 for(
unsigned int i=0; i<rbtList.size(); i++)
474 rbtList[i].computeLimbKinematic();
476 rbtList[i].getKinematic(w,dw,ddp);
486 for(
unsigned int i=0; i<rbtList.size(); i++)
491 rbtList[i].setKinematic(w,dw,ddp);
493 rbtList[i].computeLimbKinematic();
503 fprintf(stderr,
"iDynNode error: there are %d limbs with Kinematic Flow = Input. Only one limb must have Kinematic Input from outside measurements/computations. \n",inputNode);
504 fprintf(stderr,
"Please check the coherence of the limb configuration in the node <%s> \n",
info.c_str());
511 bool iDynNode::solveKinematics(
const Vector &w0,
const Vector &dw0,
const Vector &ddp0)
513 unsigned int inputNode=0;
517 for(
unsigned int i=0; i<rbtList.size(); i++)
521 rbtList[i].setKinematicMeasure(w0,dw0,ddp0);
523 rbtList[i].computeLimbKinematic();
525 rbtList[i].getKinematic(w,dw,ddp);
535 for(
unsigned int i=0; i<rbtList.size(); i++)
540 rbtList[i].setKinematic(w,dw,ddp);
542 rbtList[i].computeLimbKinematic();
552 fprintf(stderr,
"iDynNode: error: there are %d limbs with Kinematic Flow = Input. ",inputNode);
553 fprintf(stderr,
" Only one limb must have Kinematic Input from outside measurements/computations. ");
554 fprintf(stderr,
"Please check the coherence of the limb configuration in the node <%s> \n",
info.c_str());
561 bool iDynNode::setKinematicMeasure(
const Vector &w0,
const Vector &dw0,
const Vector &ddp0)
563 if( (w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3))
567 for(
unsigned int i=0; i<rbtList.size(); i++)
571 rbtList[i].setKinematicMeasure(w0,dw0,ddp0);
576 fprintf(stderr,
"iDynNode: error, there is not a node to set kinematic measure! \n");
583 fprintf(stderr,
"iDynNode: error, could not set Kinematic measures due to wrong sized vectors: \n");
584 fprintf(stderr,
" w,dw,ddp have length %d,%d,%d instead of 3,3,3. \n",(
int)w0.length(),(
int)dw0.length(),(
int)ddp0.length());
590 bool iDynNode::solveWrench()
592 unsigned int outputNode = 0;
598 for(
unsigned int i=0; i<rbtList.size(); i++)
603 rbtList[i].computeLimbWrench();
607 rbtList[i].getWrench(
F,Mu);
620 if(outputNode==rbtList.size())
624 fprintf(stderr,
"iDynNode: warning: there are no limbs with Wrench Flow = Output. ");
625 fprintf(stderr,
" At least one limb must have Wrench Output for balancing forces in the node. ");
626 fprintf(stderr,
"Please check the coherence of the limb configuration in the node <%s> \n",
info.c_str());
631 for(
unsigned int i=0; i<rbtList.size(); i++)
636 rbtList[i].setWrench(
F,Mu);
638 rbtList[i].computeLimbWrench();
644 bool iDynNode::solveWrench(
const Matrix &FM)
646 bool inputWasOk = setWrenchMeasure(FM);
648 return ( solveWrench() && inputWasOk );
651 bool iDynNode::solveWrench(
const Matrix &Fm,
const Matrix &Mm)
653 bool inputWasOk = setWrenchMeasure(Fm,Mm);
655 return ( solveWrench() && inputWasOk );
658 bool iDynNode::setWrenchMeasure(
const Matrix &FM)
660 Vector fi(3); fi.zero();
661 Vector mi(3); mi.zero();
662 Vector FMi(6);FMi.zero();
663 bool inputWasOk =
true;
666 int inputNode = howManyWrenchInputs(
false);
675 if(FM.cols()<inputNode)
679 fprintf(stderr,
"iDynNode: could not solveWrench due to missing wrenches to initialize the computations: ");
680 fprintf(stderr,
" only %zu f/m available instead of %d. Using default values, all zero. \n ",FM.cols(),inputNode);
688 fprintf(stderr,
"iDynNode: could not solveWrench due to wrong sized init wrenches: ");
689 fprintf(stderr,
" %zu instead of 6 (3+3). Using default values, all zero. \n",FM.rows());
698 for(
unsigned int i=0; i<rbtList.size(); i++)
703 FMi = FM.getCol(inputNode);
704 fi[0]=FMi[0];fi[1]=FMi[1];fi[2]=FMi[2];
705 mi[0]=FMi[3];mi[1]=FMi[4];mi[2]=FMi[5];
708 rbtList[i].setWrenchMeasure(fi,mi);
715 for(
unsigned int i=0; i<rbtList.size(); i++)
717 rbtList[i].setWrenchMeasure(fi,mi);
724 bool iDynNode::setWrenchMeasure(
const Matrix &Fm,
const Matrix &Mm)
726 bool inputWasOk =
true;
729 int inputNode = howManyWrenchInputs(
false);
738 if((Fm.cols()<inputNode)||(Mm.cols()<inputNode))
742 fprintf(stderr,
"iDynNode: could not setWrenchMeasure due to missing wrenches to initialize the computations: ");
743 fprintf(stderr,
" only %zu/%zu f/m available instead of %d/%d. Using default values, all zero. \n",Fm.cols(),Mm.cols(),inputNode,inputNode);
747 if((Fm.rows()!=3)||(Mm.rows()!=3))
751 fprintf(stderr,
"iDynNode: could not setWrenchMeasure due to wrong sized init f/m: ");
752 fprintf(stderr,
" %zu/%zu instead of 3/3. Using default values, all zero. \n",Fm.rows(),Mm.rows());
761 for(
unsigned int i=0; i<rbtList.size(); i++)
767 rbtList[i].setWrenchMeasure(Fm.getCol(inputNode),Mm.getCol(inputNode));
775 Vector fi(3), mi(3); fi.zero(); mi.zero();
776 for(
unsigned int i=0; i<rbtList.size(); i++)
778 rbtList[i].setWrenchMeasure(fi,mi);
785 unsigned int iDynNode::howManyWrenchInputs(
bool afterAttach)
const
787 unsigned int inputNode = 0;
790 for(
unsigned int i=0; i<rbtList.size(); i++)
796 if(afterAttach==
true) inputNode--;
801 unsigned int iDynNode::howManyKinematicInputs(
bool afterAttach)
const
803 unsigned int inputNode = 0;
806 for(
unsigned int i=0; i<rbtList.size(); i++)
812 if(afterAttach==
true) inputNode--;
817 Vector iDynNode::getForce()
const {
return F;}
819 Vector iDynNode::getMoment()
const {
return Mu;}
821 Vector iDynNode::getAngVel()
const {
return w;}
823 Vector iDynNode::getAngAcc()
const {
return dw;}
825 Vector iDynNode::getLinAcc()
const {
return ddp;}
834 Matrix iDynNode::computeJacobian(
unsigned int iChain)
836 if(iChain<=rbtList.size())
838 return rbtList[iChain].computeGeoJacobian(
false);
842 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to out of range index: input limb has index %d > %d. Returning a null matrix. \n",iChain,(
int)rbtList.size());
847 Matrix iDynNode::computeJacobian(
unsigned int iChain,
unsigned int iLink)
849 if(iChain<=rbtList.size())
853 return rbtList[iChain].computeGeoJacobian(iLink,
false);
857 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to out of range index: input limb has index %d > %d. Returning a null matrix. \n",iChain,(
int)rbtList.size());
862 Matrix iDynNode::computeJacobian(
unsigned int iChainA,
JacobType dirA,
unsigned int iChainB,
JacobType dirB)
866 if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
868 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix. \n",iChainA,iChainB,(
int)rbtList.size());
872 if( iChainA==iChainB )
874 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to weird index for chains %d: same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix. \n",iChainA);
882 Matrix J(6,rbtList[iChainA].getDOF() + rbtList[iChainB].getDOF()); J.zero();
886 Matrix J_A; Matrix J_B;
893 compute_Pn_HAN(iChainA, dirA, iChainB, dirB, Pn, H_A_Node);
897 J_A = rbtList[iChainA].computeGeoJacobian(Pn,
false);
900 J_B = rbtList[iChainB].computeGeoJacobian(Pn,H_A_Node,
false);
904 unsigned int JAcols = J_A.cols();
905 unsigned int JBcols = J_B.cols();
907 if(JAcols+JBcols!=J.cols())
909 fprintf(stderr,
"iDynNode error: Jacobian should be 6x%zu instead is 6x%d \n",J.cols(),(JAcols+JBcols));
910 fprintf(stderr,
"Note: limb A: N=%d DOF=%d \n",rbtList[iChainA].getNLinks(),rbtList[iChainA].getDOF());
911 fprintf(stderr,
"Note: limb B: N=%d DOF=%d \n",rbtList[iChainB].getNLinks(),rbtList[iChainB].getDOF());
912 J.resize(6,JAcols+JBcols);
915 for(
unsigned int r=0; r<6; r++)
917 for(c=0;c<JAcols;c++)
919 for(c=0;c<JBcols;c++)
920 J(r,JAcols+c)=J_B(r,c);
927 Matrix iDynNode::computeJacobian(
unsigned int iChainA,
JacobType dirA,
unsigned int iChainB,
unsigned int iLinkB,
JacobType dirB)
931 if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
933 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix.\n",iChainA,iChainB,(
int)rbtList.size());
937 if( iChainA==iChainB )
939 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to weird index for chains %d : same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix.\n",iChainA);
943 if( iLinkB >= rbtList[iChainB].getNLinks())
945 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to out of range index for chain %d: the selected link %d > %d. Returning a null matrix. \n",iChainB,iLinkB,rbtList[iChainB].getNLinks());
954 Matrix J(6,rbtList[iChainA].getDOF() + iLinkB ); J.zero();
958 Matrix J_A; Matrix J_B;
965 compute_Pn_HAN(iChainA, dirA, iChainB, iLinkB, dirB, Pn, H_A_Node);
969 J_A = rbtList[iChainA].computeGeoJacobian(Pn,
false);
972 J_B = rbtList[iChainB].computeGeoJacobian(iLinkB,Pn,H_A_Node,
false);
976 unsigned int JAcols = J_A.cols();
977 unsigned int JBcols = J_B.cols();
979 if(JAcols+JBcols!=J.cols())
981 fprintf(stderr,
"iDynNode error: Jacobian should be 6x%zu instead is 6x%d \n",J.cols(),(JAcols+JBcols));
982 fprintf(stderr,
"Note: limb A: N=%d DOF=%d \n",rbtList[iChainA].getNLinks(),rbtList[iChainA].getDOF());
983 fprintf(stderr,
"Note: limb B: N=%d DOF=%d iLinkB=%d \n",rbtList[iChainB].getNLinks(),rbtList[iChainB].getDOF(),iLinkB);
984 J.resize(6,JAcols+JBcols);
987 for(
unsigned int r=0; r<6; r++)
989 for(c=0;c<JAcols;c++)
991 for(c=0;c<JBcols;c++)
992 J(r,JAcols+c)=J_B(r,c);
999 void iDynNode::compute_Pn_HAN(
unsigned int iChainA,
JacobType dirA,
unsigned int iChainB,
JacobType dirB, Matrix &Pn, Matrix &H_A_Node)
1007 H_A_Node = rbtList[iChainA].getH() * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1012 H_A_Node = SE3inv(rbtList[iChainA].getH()) * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1020 Pn = H_A_Node * rbtList[iChainB].getH();
1025 Pn = H_A_Node * SE3inv(rbtList[iChainB].getH());
1029 void iDynNode::compute_Pn_HAN(
unsigned int iChainA,
JacobType dirA,
unsigned int iChainB,
unsigned int iLinkB,
JacobType dirB, Matrix &Pn, Matrix &H_A_Node)
1037 H_A_Node = rbtList[iChainA].getH() * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1042 H_A_Node = SE3inv(rbtList[iChainA].getH()) * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1053 Pn = H_A_Node * rbtList[iChainB].getH(iLinkB,
true);
1058 Pn = H_A_Node * SE3inv(rbtList[iChainB].getH(iLinkB,
true));
1062 Vector iDynNode::computePose(
unsigned int iChainA,
JacobType dirA,
unsigned int iChainB,
JacobType dirB,
const bool axisRep)
1066 if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
1068 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computePose() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix. \n",iChainA,iChainB,(
int)rbtList.size());
1072 if( iChainA==iChainB )
1074 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computePose() due to weird index for chains %d: same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix. \n",iChainA);
1081 Matrix Pn, H_A_Node;
1088 compute_Pn_HAN(iChainA, dirA, iChainB, dirB, Pn, H_A_Node);
1094 Vector r=dcm2axis(Pn);
1106 Vector r(3); r.zero();
1108 r[0]=atan2(-Pn(2,1),Pn(2,2));
1110 r[2]=atan2(-Pn(1,0),Pn(0,0));
1122 Vector iDynNode::computePose(
unsigned int iChainA,
JacobType dirA,
unsigned int iChainB,
unsigned int iLinkB,
JacobType dirB,
const bool axisRep)
1126 if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
1128 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computePose() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix. \n",iChainA,iChainB,(
int)rbtList.size());
1132 if( iChainA==iChainB )
1134 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computePose() due to weird index for chains %d: same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix. \n",iChainA);
1138 if( iLinkB >= rbtList[iChainB].getNLinks())
1140 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computePose() due to out of range index for chain %d: the selected link is %d > %d. Returning a null matrix. \n",iChainB,iLinkB,rbtList[iChainB].getNLinks());
1147 Matrix Pn, H_A_Node;
1154 compute_Pn_HAN(iChainA, dirA, iChainB, iLinkB, dirB, Pn, H_A_Node);
1160 Vector r=dcm2axis(Pn);
1172 Vector r(3); r.zero();
1174 r[0]=atan2(-Pn(2,1),Pn(2,2));
1176 r[2]=atan2(-Pn(1,0),Pn(0,0));
1194 Matrix iDynNode::TESTING_computeCOMJacobian(
unsigned int iChain,
unsigned int iLink)
1196 if(iChain<=rbtList.size())
1200 return rbtList[iChain].TESTING_computeCOMJacobian(iLink,
false);
1204 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeCOMJacobian() due to out of range index: input limb has index %d > %d. Returning a null matrix. \n",iChain,(
int)rbtList.size());
1209 Matrix iDynNode::TESTING_computeCOMJacobian(
unsigned int iChainA,
JacobType dirA,
unsigned int iChainB,
unsigned int iLinkB,
JacobType dirB)
1213 if( (iChainA > rbtList.size())||(iChainB > rbtList.size()) )
1215 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to out of range index: limbs have index %d,%d > %d. Returning a null matrix. \n",iChainA,iChainB,(
int)rbtList.size());
1219 if( iChainA==iChainB )
1221 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to weird index for chains %d: same chains are selected. Please check the indexes or use the method iDynNode::computeJacobian(unsigned int iChain). Returning a null matrix. \n",iChainA);
1225 if( iLinkB >= rbtList[iChainB].getNLinks())
1227 if(verbose)
fprintf(stderr,
"iDynNode: error, could not computeJacobian() due to out of range index for chain %d: the selected link is %d > %d. Returning a null matrix. \n",iChainB,iLinkB,rbtList[iChainB].getNLinks());
1236 Matrix J(6,rbtList[iChainA].getDOF() + iLinkB ); J.zero();
1240 Matrix J_A; Matrix J_B;
1247 compute_Pn_HAN_COM(iChainA, dirA, iChainB, iLinkB, dirB, Pn, H_A_Node);
1251 J_A = rbtList[iChainA].computeGeoJacobian(Pn,
false);
1254 J_B = rbtList[iChainB].TESTING_computeCOMJacobian(iLinkB,Pn,H_A_Node,
false);
1258 unsigned int JAcols = J_A.cols();
1259 unsigned int JBcols = J_B.cols();
1261 if(JAcols+JBcols!=J.cols())
1263 fprintf(stderr,
"iDynNode error: Jacobian should be 6x%zu instead is 6x%d \n",J.cols(),(JAcols+JBcols));
1264 fprintf(stderr,
"Note: limb A: N=%d DOF=%d \n",rbtList[iChainA].getNLinks(),rbtList[iChainA].getDOF());
1265 fprintf(stderr,
"Note: limb B: N=%d DOF=%d iLinkB=%d \n",rbtList[iChainB].getNLinks(),rbtList[iChainB].getDOF(),iLinkB);
1266 J.resize(6,JAcols+JBcols);
1269 for(
unsigned int r=0; r<6; r++)
1271 for(c=0;c<JAcols;c++)
1273 for(c=0;c<JBcols;c++)
1274 J(r,JAcols+c)=J_B(r,c);
1281 void iDynNode::compute_Pn_HAN_COM(
unsigned int iChainA,
JacobType dirA,
unsigned int iChainB,
unsigned int iLinkB,
JacobType dirB, Matrix &Pn, Matrix &H_A_Node)
1289 H_A_Node = rbtList[iChainA].getH() * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1294 H_A_Node = SE3inv(rbtList[iChainA].getH()) * rbtList[iChainA].getRBT().transposed() * rbtList[iChainB].getRBT();
1305 Pn = H_A_Node * rbtList[iChainB].getHCOM(iLinkB);
1310 Pn = H_A_Node * SE3inv(rbtList[iChainB].getHCOM(iLinkB));
1354 unsigned int outputNode = 0;
1356 F.zero();
Mu.zero();
1361 for(
unsigned int i=0; i<
rbtList.size(); i++)
1368 if(
rbtList[i].isSensorized()==
true)
1369 sensorList[i]->computeWrenchFromSensorNewtonEuler();
1371 rbtList[i].computeLimbWrench();
1389 if(outputNode==
rbtList.size())
1393 fprintf(stderr,
"iDynNode: warning: there are no limbs with Wrench Flow = Output. At least one limb must have Wrench Output for balancing forces in the node. \n");
1394 fprintf(stderr,
"Please check the coherence of the limb configuration in the node <%s>\n",
info.c_str());
1400 for(
unsigned int i=0; i<
rbtList.size(); i++)
1407 rbtList[i].computeLimbWrench();
1415 Vector fi(3); fi.zero();
1416 Vector mi(3); mi.zero();
1417 Vector FMi(6);FMi.zero();
1418 bool inputWasOk =
true;
1430 if(FM.cols()<inputNode)
1434 fprintf(stderr,
"iDynNode: could not setWrenchMeasure due to missing wrenches to initialize the computations: only %zu f/m available instead of %d. Using default values, all zero.\n",FM.cols(),inputNode);
1435 if(afterAttach==
true)
1436 fprintf(stderr,
" Remember that the first limb receives wrench input during an attach from another node. \n");
1443 fprintf(stderr,
"iDynNode: could not setWrenchMeasure due to wrong sized init wrenches: %zu instead of 6 (3+3). Using default values, all zero.\n",FM.rows());
1450 unsigned int startLimb=0;
1451 if(afterAttach) startLimb=1;
1457 for(
unsigned int i=startLimb; i<
rbtList.size(); i++)
1462 FMi = FM.getCol(inputNode);
1463 fi[0]=FMi[0];fi[1]=FMi[1];fi[2]=FMi[2];
1464 mi[0]=FMi[3];mi[1]=FMi[4];mi[2]=FMi[5];
1469 if(
rbtList[i].isSensorized()==
true)
1472 rbtList[i].setWrenchMeasure(fi,mi);
1479 for(
unsigned int i=startLimb; i<
rbtList.size(); i++)
1482 if(
rbtList[i].isSensorized()==
true)
1485 rbtList[i].setWrenchMeasure(fi,mi);
1495 bool inputWasOk =
true;
1507 if((Fm.cols()<inputNode)||(Mm.cols()<inputNode))
1511 fprintf(stderr,
"iDynNode: could not setWrenchMeasure due to missing wrenches to initialize the computations: only %zu/%zu f/m available instead of %d/%d. Using default values, all zero.\n",Fm.cols(),Mm.cols(),inputNode,inputNode);
1512 if(afterAttach==
true)
1513 fprintf(stderr,
" Remember that the first limb receives wrench input during an attach from another node.\n");
1517 if((Fm.rows()!=3)||(Mm.rows()!=3))
1519 if(
verbose)
fprintf(stderr,
"iDynNode: could not setWrenchMeasure due to wrong sized init f/m: %zu/%zu instead of 3/3. Using default values, all zero.\n",Fm.rows(),Mm.rows());
1526 unsigned int startLimb=0;
1527 if(afterAttach) startLimb=1;
1533 for(
unsigned int i=startLimb; i<
rbtList.size(); i++)
1541 if(
rbtList[i].isSensorized()==
true)
1542 rbtList[i].setWrenchMeasure(
sensorList[i],Fm.getCol(inputNode),Mm.getCol(inputNode));
1544 rbtList[i].setWrenchMeasure(Fm.getCol(inputNode),Mm.getCol(inputNode));
1552 Vector fi(3), mi(3); fi.zero(); mi.zero();
1553 for(
unsigned int i=startLimb; i<
rbtList.size(); i++)
1555 rbtList[i].setWrenchMeasure(fi,mi);
1564 unsigned int inputNode = 0;
1565 unsigned int outputNode = 0;
1566 unsigned int numSensor = 0;
1567 bool inputWasOk =
true;
1568 Vector fi(3); fi.zero();
1569 Vector mi(3); mi.zero();
1570 Vector FMi(6);FMi.zero();
1574 F.zero();
Mu.zero();
1585 if(
verbose)
fprintf(stderr,
"iDynSensorNode: could not setWrenchMeasure due to wrong sized init wrenches matrix: %zu rows instead of 6 (3+3). Using default values, all zero.\n",FM.rows());
1588 if(FM.cols()!=inputNode)
1591 fprintf(stderr,
"iDynSensorNode: could not setWrenchMeasure due to wrong sized init wrenches: %zu instead of %d. Using default values, all zero. \n",FM.cols(),inputNode);
1592 if(afterAttach==
true)
1593 fprintf(stderr,
" Remember that the first limb receives wrench input during an attach from another node.\n");
1600 unsigned int startLimb=0;
1601 if(afterAttach) startLimb=1;
1606 for(
unsigned int i=startLimb; i<
rbtList.size(); i++)
1611 FMi = FM.getCol(inputNode);
1612 fi[0]=FMi[0];fi[1]=FMi[1];fi[2]=FMi[2];
1613 mi[0]=FMi[3];mi[1]=FMi[4];mi[2]=FMi[5];
1617 rbtList[i].setWrenchMeasure(fi,mi);
1624 for(
unsigned int i=startLimb; i<
rbtList.size(); i++)
1626 rbtList[i].setWrenchMeasure(fi,mi);
1632 for(
unsigned int i=0; i<
rbtList.size(); i++)
1638 rbtList[i].computeLimbWrench();
1656 if(outputNode==
rbtList.size())
1660 fprintf(stderr,
"iDynSensorNode: warning: there are no limbs with Wrench Flow = Output. ");
1661 fprintf(stderr,
" At least one limb must have Wrench Output for balancing forces in the node. ");
1662 fprintf(stderr,
"Please check the coherence of the limb configuration in the node <%s> \n",
info.c_str());
1668 for(
unsigned int i=0; i<
rbtList.size(); i++)
1675 rbtList[i].computeLimbWrench();
1681 for(
unsigned int i=0; i<
rbtList.size(); i++)
1685 if(
rbtList[i].isSensorized()==
true)
1695 ret.resize(6,numSensor);
1697 for(
unsigned int i=0; i<
rbtList.size(); i++)
1698 if(
rbtList[i].isSensorized()==
true)
1700 ret.setCol(numSensor,
sensorList[i]->getSensorForceMoment());
1709 unsigned int numSensor = 0;
1710 for(
unsigned int i=0; i<
rbtList.size(); i++)
1712 if(
rbtList[i].isSensorized()==
true)
1745 delete up;
up = NULL;
1760 Matrix FM(6,2); FM.zero();
1761 if((FM_right.length()==6)&&(FM_left.length()==6))
1763 FM.setCol(0,FM_right);
1764 FM.setCol(1,FM_left);
1769 if(
verbose)
fprintf(stderr,
"Node <%s> could not set sensor measurements properly due to wrong sized vectors. FM right/left have length %d,%d instead of 6,6. Setting everything to zero. \n",
name.c_str(),(
int)FM_right.length(),(
int)FM_left.length());
1777 Matrix FM(6,3); FM.zero();
1778 if((FM_right.length()==6)&&(FM_left.length()==6)&&(FM_up.length()==6))
1782 FM.setCol(1,FM_right);
1783 FM.setCol(2,FM_left);
1788 if(
verbose)
fprintf(stderr,
"Node <%s> could not set sensor measurements properly due to wrong sized vectors. FM up/right/left have length %d,%d,%d instead of 6,6. Setting everything to zero. \n",
name.c_str(),(
int)FM_up.length(),(
int)FM_right.length(),(
int)FM_left.length());
1802 bool iDynSensorTorsoNode::update(
const Vector &w0,
const Vector &dw0,
const Vector &ddp0,
const Vector &FM_right,
const Vector &FM_left,
const Vector &FM_up)
1804 bool inputOk =
true;
1806 if((FM_right.length()==6)&&(FM_left.length()==6)&&(FM_up.length()==6)&&(w0.length()==3)&&(dw0.length()==3)&&(ddp0.length()==3))
1816 fprintf(stderr,
"Node <%s> error, could not update() due to wrong sized vectors. ",
name.c_str());
1817 fprintf(stderr,
" w0,dw0,ddp0 have size %d,%d,%d instead of 3,3,3. \n",(
int)w0.length(),(
int)dw0.length(),(
int)ddp0.length());
1818 fprintf(stderr,
" FM up/right/left have size %d,%d,%d instead of 6,6,6. \n",(
int)FM_up.length(),(
int)FM_right.length(),(
int)FM_left.length());
1819 fprintf(stderr,
" Updating without new values.\n");
1828 if(afterAttach==
false)
1832 fprintf(stderr,
"Node <%s> error, could not update() due to missing wrench vectors. ",
name.c_str());
1833 fprintf(stderr,
"This type of update() only works after an attachTorso() or after having set the central limb wrench and kinematics variables. ");
1834 fprintf(stderr,
"You should try with the other update() methods. ");
1835 fprintf(stderr,
"Could not perform update(): exiting. \n");
1840 if((FM_right.length()==6)&&(FM_left.length()==6))
1843 FM.setCol(0,FM_right);
1844 FM.setCol(1,FM_left);
1852 fprintf(stderr,
"Node <%s> error, could not update() due to wrong sized vectors. ",
name.c_str());
1853 fprintf(stderr,
"FM right/left have size %d,%d instead of 6,6. Updating without new values. \n",(
int)FM_right.length(),(
int)FM_left.length());
1915 yarp::sig::Vector COM_UP(4); COM_UP.zero();
1916 yarp::sig::Vector COM_RT(4); COM_RT.zero();
1917 yarp::sig::Vector COM_LF(4); COM_LF.zero();
1926 yarp::sig::Matrix *orig = 0;
1927 yarp::sig::Vector *total_COM = 0;
1928 double *total_mass = 0;
1930 for (
int n=0;
n<3;
n++)
1935 limb = (this->
left);
1936 orig = &(this->
HLeft);
1941 limb = (this->
right);
1948 orig = &(this->
HUp);
1954 for (i=0; i<limb->
getN(); i++)
1956 (*total_mass)=(*total_mass)+limb->
getMass(i);
1958 yarp::sig::Matrix m = limb->
getH(i,
true);
1959 (*total_COM) = (*total_COM) + ((*orig) * m *
COM) * limb->
getMass(i);
1961 if (fabs(*total_mass) > 0.00001)
1962 {(*total_COM)=(*total_COM)/(*total_mass); }
1964 {(*total_COM).zero();}
1973 yarp::sig::Matrix *orig = 0;
1974 yarp::sig::Matrix *COM_jacob = 0;
1975 double *total_mass = 0;
1978 for (
int n=0;
n<3;
n++)
1983 limb = (this->
left);
1984 orig = &(this->
HLeft);
1989 limb = (this->
right);
1996 orig = &(this->
HUp);
2003 double* partial_mass = 0;
2004 Vector* partial_COM = 0;
2005 partial_mass =
new double [limb->
getN()];
2006 partial_COM =
new Vector [limb->
getN()];
2007 COM_jacob->resize(6,limb->
getN());
2011 intH.push_back(*orig);
2012 for (
unsigned int i=0; i<limb->
getN(); i++)
2013 intH.push_back((*orig)*limb->
getH(i,
true));
2015 for (
unsigned int iLink=0; iLink<limb->
getN(); iLink++)
2017 partial_mass[iLink]=0;
2018 partial_COM [iLink].resize(4);
2019 partial_COM [iLink].zero();
2022 yarp::sig::Matrix m = (*orig) * limb->
getH(iLink,
true);
2025 for (
unsigned int i=iLink; i<limb->
getN(); i++)
2027 yarp::sig::Matrix m_i = (*orig) * limb->
getH(i,
true);
2028 partial_COM[iLink] =partial_COM[iLink] + m_i * limb->
getCOM(i).getCol(3) * limb->
getMass(i);
2029 partial_mass[iLink]=partial_mass[iLink]+limb->
getMass(i);
2030 if(partial_mass[iLink]==0) partial_mass[iLink]=1
e-15;
2032 partial_COM[iLink] = (partial_COM[iLink]/partial_mass[iLink])-intH[iLink].getCol(3);
2035 printf (
"parCOM: %d %+3.3f %+3.3f %+3.3f \n", iLink, partial_COM[iLink](0), partial_COM[iLink](1), partial_COM[iLink](2));
2036 printf (
"n : %d %+3.3f %+3.3f %+3.3f \n", iLink, intH[iLink].getCol(3)(0),intH[iLink].getCol(3)(1),intH[iLink].getCol(3)(2));
2037 printf (
"p-nCOM: %d %+3.3f %+3.3f %+3.3f \n", iLink, partial_COM[iLink](0), partial_COM[iLink](1), partial_COM[iLink](2));
2040 double mass_coeff = partial_mass[iLink]/(*total_mass);
2042 Vector Z = intH[iLink].subcol(0,2,3);
2043 Vector
w =
cross(Z,partial_COM[iLink].subVector(0,2));
2045 (*COM_jacob)(0,iLink)=mass_coeff*
w[0];
2046 (*COM_jacob)(1,iLink)=mass_coeff*
w[1];
2047 (*COM_jacob)(2,iLink)=mass_coeff*
w[2];
2048 (*COM_jacob)(3,iLink)=Z(0);
2049 (*COM_jacob)(4,iLink)=Z(1);
2050 (*COM_jacob)(5,iLink)=Z(2);
2053 delete [] partial_mass;
2054 delete [] partial_COM;
2260 unsigned int SENSOR_LINK_INDEX = 2;
2267 HUp.resize(4,4);
HUp.eye();
2300 name =
"upper_torso";
2329 unsigned int SENSOR_LINK_INDEX = 1;
2355 HUp.resize(4,4);
HUp.zero();
2384 name =
"lower_torso";
2429 Vector out_w; out_w.resize(3);
2430 Vector out_dw; out_dw.resize(3);
2431 Vector out_ddp; out_ddp.resize(3);
2432 Vector out_F; out_F.resize(3);
2433 Vector out_M; out_M.resize(3);
2440 out_dw[0]= in_dw[0];
2441 out_dw[1]= in_dw[1];
2442 out_dw[2]= in_dw[2];
2444 out_ddp[0]= in_ddp[0];
2445 out_ddp[1]= in_ddp[1];
2446 out_ddp[2]= in_ddp[2];
2515 yarp::sig::Matrix T0;
2516 yarp::sig::Matrix T1;
2517 yarp::sig::Matrix hright;
2537 #ifdef DEBUG_FOOT_COM
2538 fprintf(stderr,
"Whole COM being computed w.r.t Foot Reference Frame... \n");
2618 for (i=0; i<6; i++, j++)
pos[j] = ll[i];
2619 for (i=0; i<6; i++, j++)
pos[j] = rl[i];
2620 for (i=0; i<3; i++, j++)
pos[j] = to[i];
2621 for (i=0; i<7; i++, j++)
pos[j] = la[i];
2622 for (i=0; i<7; i++, j++)
pos[j] = ra[i];
2623 for (i=0; i<3; i++, j++)
pos[j] = hd[i];
2640 for (i=0; i<6; i++, j++) vel[j] = ll[i];
2641 for (i=0; i<6; i++, j++) vel[j] = rl[i];
2642 for (i=0; i<3; i++, j++) vel[j] = to[i];
2643 for (i=0; i<7; i++, j++) vel[j] = la[i];
2644 for (i=0; i<7; i++, j++) vel[j] = ra[i];
2645 for (i=0; i<3; i++, j++) vel[j] = hd[i];
2700 for (c=0; c<6; c++, ct++)
2703 for (c=0; c<6; c++, ct++)
2706 for (c=0; c<3; c++, ct++)
2711 tmp_Jac.resize(6,32);
2721 T_ad = adjoint(T0*T1);
2722 Matrix RotZero; RotZero.resize(3,3); RotZero.zero();
2723 T_ad.setSubmatrix(RotZero,0,3);
2737 tmp_Jac.setSubmatrix(T_ad*COMHead.submatrix(0,5,0,2),0,29);
2750 Matrix RotZero; RotZero.resize(3,3); RotZero.zero();
2751 T_rotT0.setSubmatrix(RotZero,0,3);
2758 Matrix H_T0T1; H_T0T1.resize(4,4); H_T0T1 = T0*T1;
2759 Matrix rotT0T1; rotT0T1.resize(3,3); rotT0T1 = H_T0T1.submatrix(0,2,0,2);
2762 Vector RcL = rotT0T1*(LFcom.subVector(0,2));
2766 Vector RcR = rotT0T1*(RTcom.subVector(0,2));
2770 Vector RcH = rotT0T1*(HDcom.subVector(0,2));
2773 Matrix L1; L1.resize(6,3); L1.zero();
2774 L1.setRow(0, -1*(Jac_Torso.getRow(5)*RcR(1)) + Jac_Torso.getRow(4)*RcR(2));
2775 L1.setRow(1, Jac_Torso.getRow(5)*RcR(0) + -1*Jac_Torso.getRow(3)*RcR(2));
2776 L1.setRow(2, -1*(Jac_Torso.getRow(4)*RcR(0)) + Jac_Torso.getRow(3)*RcR(1));
2778 Matrix L2; L2.resize(6,3); L2.zero();
2779 L2.setRow(0, -1*(Jac_Torso.getRow(5)*RcL(1)) + Jac_Torso.getRow(4)*RcL(2));
2780 L2.setRow(1, Jac_Torso.getRow(5)*RcL(0) + -1*Jac_Torso.getRow(3)*RcL(2));
2781 L2.setRow(2, -1*(Jac_Torso.getRow(4)*RcL(0)) + Jac_Torso.getRow(3)*RcL(1));
2783 Matrix L3; L3.resize(6,3); L3.zero();
2784 L3.setRow(0, -1*(Jac_Torso.getRow(5)*RcH(1)) + Jac_Torso.getRow(4)*RcH(2));
2785 L3.setRow(1, Jac_Torso.getRow(5)*RcH(0) + -1*Jac_Torso.getRow(3)*RcH(2));
2786 L3.setRow(2, -1*(Jac_Torso.getRow(4)*RcH(0)) + Jac_Torso.getRow(3)*RcH(1));
2790 hright.resize(4,4); hright.zero();
2795 hright(2,3)=-0.1199;hright(1,3)=0.0681;
2797 Matrix rRf; rRf.resize(6,6); rRf.zero();
2798 Matrix rTf; rTf.resize(4,4); rTf.zero();
2799 Matrix fTr; fTr.resize(4,4); fTr.zero();
2800 Matrix fLr; fLr.resize(6,32); fLr.zero();
2801 Matrix fRr; fRr.resize(6,6); fRr.zero();
2802 Vector rCw; rCw.resize(3); rCw.zero();
2804 Vector fCw; fCw.resize(3); fCw.zero();
2806 Matrix fJr; fJr.resize(6,32); fJr.zero();
2807 Matrix rJf; rJf.resize(6,32); rJf.zero();
2808 Matrix HH; HH.resize(6,32); HH.zero();
2809 Matrix LL; LL.resize(3,3); LL.zero();
2810 Matrix hright_rot6x6; hright_rot6x6.resize(6,6); hright_rot6x6.zero();
2812 #ifdef DEBUG_FOOT_COM
2820 hright_rot6x6.setSubmatrix(hright.submatrix(0,2,0,2), 0,0);
2821 hright_rot6x6.setSubmatrix(hright.submatrix(0,2,0,2), 3,3);
2823 rRf.setSubmatrix(rTf.submatrix(0,2,0,2), 0,0);
2824 rRf.setSubmatrix(rTf.submatrix(0,2,0,2), 3,3);
2832 LL(0,0) = rRf(1,0)*rTf(2,3) - rRf(2,0)*rTf(1,3);
2833 LL(0,1) = rRf(2,0)*rTf(0,3) - rRf(0,0)*rTf(2,3);
2834 LL(0,2) = rRf(0,0)*rTf(1,3) - rRf(1,0)*rTf(0,3);
2836 LL(1,0) = rRf(1,1)*rTf(2,3) - rRf(2,1)*rTf(1,3);
2837 LL(1,1) = rRf(2,1)*rTf(0,3) - rRf(0,1)*rTf(2,3);
2838 LL(1,2) = rRf(0,1)*rTf(1,3) - rRf(1,1)*rTf(0,3);
2840 LL(2,0) = rRf(1,2)*rTf(2,3) - rRf(2,2)*rTf(1,3);
2841 LL(2,1) = rRf(2,2)*rTf(0,3) - rRf(0,2)*rTf(2,3);
2842 LL(2,2) = rRf(0,2)*rTf(1,3) - rRf(1,2)*rTf(0,3);
2845 HH.setSubrow( LL(0,0)*rJf.getRow(3) + LL(0,1)*rJf.getRow(4) + LL(0,2)*rJf.getRow(5) , 0,6);
2846 HH.setSubrow( LL(1,0)*rJf.getRow(3) + LL(1,1)*rJf.getRow(4) + LL(1,2)*rJf.getRow(5) , 1,6);
2847 HH.setSubrow( LL(2,0)*rJf.getRow(3) + LL(2,1)*rJf.getRow(4) + LL(2,2)*rJf.getRow(5) , 2,6);
2850 fJr = -1*HH - (rRf.transposed())*rJf;
2855 fTr.setSubmatrix(rTf.submatrix(0,2,0,2).transposed() ,0,0);
2856 fTr.setSubcol(-1*fTr.submatrix(0,2,0,2)*rTf.subcol(0,3,3) ,0,3);
2859 fRr.setSubmatrix(fTr.submatrix(0,2,0,2),0,0);
2860 fRr.setSubmatrix(fTr.submatrix(0,2,0,2),3,3);
2862 fCw = fTr.submatrix(0,2,0,2)*rCw;
2863 fLr.setSubrow(-fCw(1)*fJr.getRow(5) + fCw(2)*fJr.getRow(4), 0,6);
2864 fLr.setSubrow( fCw(0)*fJr.getRow(5) - fCw(2)*fJr.getRow(3), 1,6);
2865 fLr.setSubrow(-fCw(0)*fJr.getRow(4) + fCw(1)*fJr.getRow(3), 2,6);
2871 Vector temp_com; temp_com.resize(4,1); temp_com.zero();
2873 unsigned int r,c,ct=0;
2874 double tmp, tmp2;
tmp = tmp2 = 0.0;
2885 for (c=0; c<3; c++, ct++){
2887 jac(r,ct) += tmp2*Jac_Torso(r,c);
2895 #ifdef DEBUG_FOOT_COM
2897 jac = fJr + fLr + fRr*jac;
2902 temp_com = fTr*temp_com;
2917 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2918 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2919 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2928 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2929 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2930 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2935 fprintf(stderr,
"COM_Jacob2\n %s\n\n",
COM_Jacob.submatrix(0,5,29,31).toString().c_str());
2944 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2945 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2946 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2947 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2948 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2955 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2957 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2958 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2959 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2960 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2967 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2968 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2970 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2971 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2972 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2979 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2980 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2981 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2983 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2984 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2991 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2992 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
2993 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
2994 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
2996 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
3003 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
3004 for (c=0; c<6; c++, ct++) jac(r,ct) = 0;
3005 for (c=0; c<3; c++, ct++) jac(r,ct) = 0;
3006 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;
3007 for (c=0; c<7; c++, ct++) jac(r,ct) = 0;