KinVec rotAcc( const Frame & f , const Frame & wt , const Frame & ei , const Expression & q , const Expression & dq , const Expression & ddq ){ IntermediateState R = eye(3); R = f.chain(R,wt,0); IntermediateState J1=jacobian(R.getRow(2).transpose(),q); IntermediateState J2=jacobian(R.getRow(0).transpose(),q); IntermediateState J3=jacobian(R.getRow(1).transpose(),q); Expression J; J.appendRows(R.getRow(1)*J1); J.appendRows(R.getRow(2)*J2); J.appendRows(R.getRow(0)*J3); J=wt.chain(J,ei,0); //IntermediateState tempJ = jacobian( J*dq, q ); //IntermediateState acnst = tempJ*dq; //IntermediateState tempJ = jacobian( J*dq, q ); IntermediateState acnst = (J*dq).ADforward(q,dq); return KinVec( J*ddq+acnst, 0, ei, J, acnst, q, dq, ddq, -2 ); }
KinVec rotVel( const Frame & f , const Frame & wt , const Frame & ei , const Expression & q , const Expression & dq , const Expression & ddq ){ IntermediateState R = eye(3); R=f.chain(R,wt,0); //Expression J; //J << R.getRow(1)*jacobian(R.getRow(2).transpose(),q); //J << R.getRow(2)*jacobian(R.getRow(0).transpose(),q); //J << R.getRow(0)*jacobian(R.getRow(1).transpose(),q); IntermediateState J1 = jacobian(R.getRow(2).transpose(),q); IntermediateState J2 = jacobian(R.getRow(0).transpose(),q); IntermediateState J3 = jacobian(R.getRow(1).transpose(),q); Expression J; J.appendRows(R.getRow(1)*J1); J.appendRows(R.getRow(2)*J2); J.appendRows(R.getRow(0)*J3); J = wt.chain(J,ei,0); return KinVec( J*dq, 0, ei, J, q, dq, ddq, -1 ); }
int main( ){ IntermediateState y0; IntermediateState y1=y0*2+1; output("sym1.txt",y1); y0=4; output("sym2.txt",y1); y0=2; output("sym3.txt",y1); Expression E; IntermediateState e1(1,3);e1(0)=1;e1(1)=2;e1(2)=3; IntermediateState e2(1,3);e2(0)=-1;e2(1)=-2;e2(2)=-3; // Thus doesn't work with DifferentialState e1(3); E.appendRows(e1); printf("Shape %d x %d\n",E.getNumRows(),E.getNumCols()); E.appendRows(e2); output("sym4.txt",E); return 0; }