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;
}
