rbdlsim/3rdparty/rbdl/tests/CustomConstraintsTests.cc

521 lines
16 KiB
C++

#include <UnitTest++.h>
#include "rbdl/rbdl.h"
#include "rbdl/Constraints.h"
#include <cassert>
#include "PendulumModels.h"
using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
const double TEST_PREC = 1.0e-11;
struct PinJointCustomConstraint : public RigidBodyDynamics::CustomConstraint
{
PinJointCustomConstraint():RigidBodyDynamics::CustomConstraint(5){
}
PinJointCustomConstraint(unsigned int x0y1z2):RigidBodyDynamics::CustomConstraint(5){
TuP.resize(mConstraintCount);
for(unsigned int i=0; i<TuP.size(); ++i){
TuP[i].setZero();
}
/*A pin joint has constraints about these axis when expressed
in the frame of the predecessor body:
0 1 2 3 4 5
wx wy wz rx ry rz
0: [ 0 1 0 0 0 0 ]
1: [ 0 0 1 0 0 0 ]
2: [ 0 0 0 1 0 0 ]
3: [ 0 0 0 0 1 0 ]
4: [ 0 0 0 0 0 1 ]
*/
switch(x0y1z2){
case 0:
{
TuP[3][1] = 1;
TuP[4][2] = 1;
}break;
case 1:{
TuP[3][0] = 1;
TuP[4][2] = 1;
}break;
case 2:{
TuP[3][0] = 1;
TuP[4][1] = 1;
}break;
default: {
cerr << "Invalid AxisOfRotation argument" << endl;
}
};
TuP[0][3] = 1;
TuP[1][4] = 1;
TuP[2][5] = 1;
}
virtual void CalcConstraintsJacobianAndConstraintAxis(
Model &model,
unsigned int ccid,
const Math::VectorNd &Q,
ConstraintSet &CS,
Math::MatrixNd &G,
unsigned int GrowStart,
unsigned int GcolStart)
{
//Set the working matrices for the previous/successor point Jacobians
//to zero
CS.GSpi.setZero();
CS.GSsi.setZero();
CalcPointJacobian6D(model,Q,CS.body_p[ccid],CS.X_p[ccid].r,CS.GSpi,false);
CalcPointJacobian6D(model,Q,CS.body_s[ccid],CS.X_s[ccid].r,CS.GSsi,false);
CS.GSJ = CS.GSsi-CS.GSpi;
r0P0 = CalcBodyToBaseCoordinates (model, Q, CS.body_p[ccid],
CS.X_p[ccid].r, false);
rmP0 = CalcBodyWorldOrientation (model, Q, CS.body_p[ccid], false
).transpose()* CS.X_p[ccid].E;
xP0 = SpatialTransform (rmP0, r0P0);
for(unsigned int i=0; i<TuP.size(); ++i){
eT0 = xP0.apply(TuP[i]);
CS.constraintAxis[ccid+i] = TuP[i]; //To keep it up to date.
G.block(GrowStart+i,GcolStart,1,model.dof_count)
= eT0.transpose()*CS.GSJ;
}
}
virtual void CalcGamma( Model &model,
unsigned int ccid,
const Math::VectorNd &Q,
const Math::VectorNd &QDot,
ConstraintSet &CS,
const MatrixNd &UNUSED(Gblock),
Math::VectorNd &gamma,
unsigned int gammaStartIndex)
{
/*
Position-level
phi(q) = 0
r0P-r0S = 0 : the points p and q are coincident.
e1x'e2y = 0 : the x axis of frame 1 is perp. to y axis of frame 2
e1x'e2z = 0 : the x axis of frame 1 is perp. to z axis of frame 2
Velocity-level
D_phi(q)_Dq * dq/dt = 0
[J_r0P0_q - J_r0Q0_q] dq/dt = 0
[J_e1x0_q'*e2y0 + e1x1'*J_e2y0_q] dq/dt = 0
[J_e1x0_q'*e2z0 + e1x1'*J_e2z0_q] dq/dt = 0
Or equivalently
Tu[vP - vS] = 0
where Tu are the directions the constraint is applied in and
vP and vQ are the spatial velocities of points P and Q. This
can be re-worked into:
G(q)*dq/dt = 0
Where G(q) is the Jacobian of the constraint phi w.r.t. q.
Acceleration-level
d/dt(Tu)[vP - vS] + Tu[aP - aS] = 0
This is equivalent to
G(q)*d^2q/dt^2 + [D_G(q)_Dq * dq/dt]*dq/dt = 0.
Gamma is the term on the right. Note that it has no d^2q/dt^2 terms.
Thus the gamma term can be computed by evaluating
Gamma = - (vP x Tu)[vP - vS] - Tu[aP* - aS*]
where aP* and aQ* are the spatial accelerations of points P and Q
evaluated with d^2q/dt^2 = 0.
*/
v0P0 = CalcPointVelocity6D(model, Q, QDot,
CS.body_p[ccid],
CS.X_p[ccid].r,false);
v0S0 = CalcPointVelocity6D(model, Q, QDot,
CS.body_s[ccid],
CS.X_s[ccid].r,false);
dv0P0nl = CalcPointAcceleration6D(model, Q, QDot,
VectorNd::Zero(model.dof_count),
CS.body_p[ccid],
CS.X_p[ccid].r,false);
dv0S0nl = CalcPointAcceleration6D(model, Q, QDot,
VectorNd::Zero(model.dof_count),
CS.body_s[ccid],
CS.X_s[ccid].r,false);
r0P0 = CalcBodyToBaseCoordinates (model, Q, CS.body_p[ccid], CS.X_p[ccid].r,
false);
rmP0 = CalcBodyWorldOrientation (model, Q, CS.body_p[ccid], false
).transpose() * CS.X_p[ccid].E;
xP0 = SpatialTransform (rmP0, r0P0);
for(unsigned int i=0; i<TuP.size(); ++i){
eT0 = xP0.apply(TuP[i]);
eT0Dot = crossm(v0P0,eT0);
gamma[i+gammaStartIndex] = -eT0.dot(dv0S0nl-dv0P0nl)
-eT0Dot.dot(v0S0-v0P0);
}
}
virtual void CalcPositionError( Model &model,
unsigned int ccid,
const Math::VectorNd &Q,
ConstraintSet &CS,
Math::VectorNd &errPos,
unsigned int errStartIndex)
{
r0P0 = CalcBodyToBaseCoordinates (model, Q, CS.body_p[ccid],
CS.X_p[ccid].r, false);
r0S0 = CalcBodyToBaseCoordinates (model, Q, CS.body_s[ccid],
CS.X_s[ccid].r, false);
rmP0 = CalcBodyWorldOrientation (model, Q, CS.body_p[ccid], false
).transpose()* CS.X_p[ccid].E;
rmS0 = CalcBodyWorldOrientation (model, Q, CS.body_s[ccid], false
).transpose()* CS.X_s[ccid].E;
rmPS = rmP0.transpose()*rmS0;
//From Davide Corradi's nice expressions for the relative
//orientation error. To do: CHECK THIS!
err[0] = -0.5 * (rmPS(1,2) - rmPS(2,1));
err[1] = -0.5 * (rmPS(2,0) - rmPS(0,2));
err[2] = -0.5 * (rmPS(0,1) - rmPS(1,0));
err.block<3,1>(3,0) = rmP0.transpose() * (r0S0 - r0P0);
for(unsigned int i=0; i < TuP.size(); ++i){
errPos[i+errStartIndex] = TuP[i].transpose()*err;
}
}
virtual void CalcVelocityError( Model &UNUSED(model),
unsigned int UNUSED(custom_constraint_id),
const Math::VectorNd &UNUSED(Q),
const Math::VectorNd &QDot,
ConstraintSet &UNUSED(CS),
const Math::MatrixNd &Gblock,
Math::VectorNd &errVel,
unsigned int errStartIndex)
{
//Since this is a time-invariant constraint the expression for
//the velocity error is quite straight forward:
errVelBlock = Gblock*QDot;
for(unsigned int i=0; i < errVelBlock.rows();++i){
errVel[errStartIndex+i] = errVelBlock[i];
}
}
std::vector < SpatialVector > TuP; //Constraint direction vectors resolved in
//the frame that P is on.
SpatialVector err, eT0, eT0Dot, v0P0, v0S0, dv0P0nl ,dv0S0nl;
Vector3d r0P0, r0S0;
Matrix3d rmP0, rmS0, rmPS;
SpatialTransform xP0;
VectorNd errVelBlock;
};
struct DoublePerpendicularPendulumCustomConstraint {
DoublePerpendicularPendulumCustomConstraint()
: model()
, cs()
, q()
, qd()
, qdd()
, tau()
, l1(1.)
, l2(1.)
, m1(1.)
, m2(1.)
, idB1(0)
, idB2(0)
, X_p1(Xtrans(Vector3d(0., 0., 0.)))
, X_s1(Xtrans(Vector3d(0., 0., 0.)))
, X_p2(Xtrans(Vector3d(0.,-l1, 0.)))
, X_s2(Xtrans(Vector3d(0., 0., 0.))){
model.gravity = Vector3d(0.,-9.81,0.);
//Planar pendulum is at 0 when it is hanging down.
// x: points to the right
// y: points up
// z: out of the page
Body link1 = Body(m1, Vector3d( 0., -l1*0.5, 0.),
Matrix3d( m1*l1*l1/3., 0., 0.,
0., m1*l1*l1/30., 0.,
0., 0., m1*l1*l1/3.));
Body link2 = Body(m2, Vector3d( l2*0.5, 0., 0.),
Matrix3d( m2*l2*l2/30., 0., 0.,
0., m2*l2*l2/3., 0.,
0., 0., m2*l2*l2/3.));
//Joint joint_free(JointTypeFloatingBase);
Joint jointEA123T123(SpatialVector(0.,0.,0.,1.,0.,0.),
SpatialVector(0.,0.,0.,0.,1.,0.),
SpatialVector(0.,0.,0.,0.,0.,1.),
SpatialVector(0.,0.,1.,0.,0.,0.),
SpatialVector(0.,1.,0.,0.,0.,0.),
SpatialVector(1.,0.,0.,0.,0.,0.));
idB1 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0. )),
jointEA123T123, link1);
idB2 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0.)),
jointEA123T123, link2);
//Make the revolute joints about the y axis using 5 constraints
//between the end points
ccPJZaxis = PinJointCustomConstraint(2);
ccPJYaxis = PinJointCustomConstraint(1);
cs.AddCustomConstraint(&ccPJZaxis, 0,idB1, X_p1, X_s1, false, 0.1);
cs.AddCustomConstraint(&ccPJYaxis,idB1,idB2, X_p2, X_s2, false, 0.1);
cs.Bind(model);
q = VectorNd::Zero(model.dof_count);
qd = VectorNd::Zero(model.dof_count);
qdd = VectorNd::Zero(model.dof_count);
tau = VectorNd::Zero(model.dof_count);
}
Model model;
ConstraintSet cs;
VectorNd q;
VectorNd qd;
VectorNd qdd;
VectorNd tau;
double l1;
double l2;
double m1;
double m2;
unsigned int idB1;
unsigned int idB2;
unsigned int idB01;
unsigned int idB02;
SpatialTransform X_p1;
SpatialTransform X_s1;
SpatialTransform X_p2;
SpatialTransform X_s2;
PinJointCustomConstraint ccPJZaxis;
PinJointCustomConstraint ccPJYaxis;
};
TEST(CustomConstraintCorrectnessTest) {
//Test to add:
// Jacobian vs. num Jacobian
DoublePerpendicularPendulumCustomConstraint dbcc
= DoublePerpendicularPendulumCustomConstraint();
DoublePerpendicularPendulumAbsoluteCoordinates dba
= DoublePerpendicularPendulumAbsoluteCoordinates();
DoublePerpendicularPendulumJointCoordinates dbj
= DoublePerpendicularPendulumJointCoordinates();
//1. Set the pendulum modeled using joint coordinates to a specific
// state and then compute the spatial acceleration of the body.
dbj.q[0] = M_PI/3.0; //About z0
dbj.q[1] = M_PI/6.0; //About y1
dbj.qd[0] = M_PI; //About z0
dbj.qd[1] = M_PI/2.0; //About y1
dbj.tau[0]= 0.;
dbj.tau[1]= 0.;
ForwardDynamics(dbj.model,dbj.q,dbj.qd,dbj.tau,dbj.qdd);
Vector3d r010 = CalcBodyToBaseCoordinates(
dbj.model,dbj.q,dbj.idB1,
Vector3d(0.,0.,0.),true);
Vector3d r020 = CalcBodyToBaseCoordinates(
dbj.model,dbj.q,dbj.idB2,
Vector3d(0.,0.,0.),true);
// Vector3d r030 = CalcBodyToBaseCoordinates(
// dbj.model,dbj.q,dbj.idB2,
// Vector3d(dbj.l2,0.,0.),true);
SpatialVector v010 = CalcPointVelocity6D(
dbj.model,dbj.q,dbj.qd,dbj.idB1,
Vector3d(0.,0.,0.),true);
SpatialVector v020 = CalcPointVelocity6D(
dbj.model,dbj.q,dbj.qd,dbj.idB2,
Vector3d(0.,0.,0.),true);
// SpatialVector v030 = CalcPointVelocity6D(
// dbj.model,dbj.q,dbj.qd,dbj.idB2,
// Vector3d(dbj.l2,0.,0.),true);
SpatialVector a010 = CalcPointAcceleration6D(
dbj.model,dbj.q,dbj.qd,dbj.qdd,
dbj.idB1,Vector3d(0.,0.,0.),true);
SpatialVector a020 = CalcPointAcceleration6D(
dbj.model,dbj.q,dbj.qd,dbj.qdd,
dbj.idB2,Vector3d(0.,0.,0.),true);
SpatialVector a030 = CalcPointAcceleration6D(
dbj.model,dbj.q,dbj.qd,dbj.qdd,
dbj.idB2,Vector3d(dbj.l2,0.,0.),true);
//2. Set the pendulum modelled using absolute coordinates to the
// equivalent state as the pendulum modelled using joint
// coordinates. Next
double qError = 1.0;
double qDotError = 1.0;
//Pefectly initialize the pendulum made with custom constraints and
//perturb the initialization a bit.
dbcc.q[0] = r010[0];
dbcc.q[1] = r010[1]+qError;
dbcc.q[2] = r010[2];
dbcc.q[3] = dbj.q[0];
dbcc.q[4] = 0;
dbcc.q[5] = 0;
dbcc.q[6] = r020[0];
dbcc.q[7] = r020[1];
dbcc.q[8] = r020[2];
dbcc.q[9] = dbj.q[0];
dbcc.q[10] = dbj.q[1];
dbcc.q[11] = 0;
dbcc.qd[0] = v010[3];
dbcc.qd[1] = v010[4]+qDotError;
dbcc.qd[2] = v010[5];
dbcc.qd[3] = dbj.qd[0];
dbcc.qd[4] = 0;
dbcc.qd[5] = 0;
dbcc.qd[6] = v020[3];
dbcc.qd[7] = v020[4];
dbcc.qd[8] = v020[5];
dbcc.qd[9] = dbj.qd[0];
dbcc.qd[10] = dbj.qd[1];
dbcc.qd[11] = 0;
VectorNd err(dbcc.cs.size());
VectorNd errd(dbcc.cs.size());
CalcConstraintsPositionError(dbcc.model,dbcc.q,dbcc.cs,err,true);
CalcConstraintsVelocityError(dbcc.model,dbcc.q,dbcc.qd,dbcc.cs,errd,true);
CHECK(err.norm() >= qError);
CHECK(errd.norm() >= qDotError);
//Solve for the initial q and qdot terms that satisfy the constraints
VectorNd qAsm,qDotAsm,w;
qAsm.resize(dbcc.q.rows());
qDotAsm.resize(dbcc.q.rows());
w.resize(dbcc.q.rows());
for(unsigned int i=0; i<w.rows();++i){
w[i] = 1.0;
}
double tol = 1e-8;
unsigned int maxIter = 100;
CalcAssemblyQ(dbcc.model,dbcc.q,dbcc.cs,qAsm,w,tol,maxIter);
for(unsigned int i=0; i<dbcc.q.rows();++i){
dbcc.q[i] = qAsm[i];
}
CalcAssemblyQDot(dbcc.model,dbcc.q,dbcc.qd,dbcc.cs,qDotAsm,w);
for(unsigned int i=0; i<dbcc.q.rows();++i){
dbcc.qd[i] = qDotAsm[i];
}
CalcConstraintsPositionError(dbcc.model,dbcc.q,dbcc.cs,err,true);
CalcConstraintsVelocityError(dbcc.model,dbcc.q,dbcc.qd,dbcc.cs,errd,true);
//The constraint errors at the position and velocity level
//must be zero before the accelerations can be tested.
for(unsigned int i=0; i<err.rows();++i){
CHECK_CLOSE(0,err[i],TEST_PREC);
}
for(unsigned int i=0; i<errd.rows();++i){
CHECK_CLOSE(0,errd[i],TEST_PREC);
}
//Evaluate the accelerations of the constrained pendulum and
//compare those to the joint-coordinate pendulum
dba.q = dbcc.q;
dba.qd= dbcc.qd;
for(unsigned int i=0; i<dbcc.tau.rows();++i){
dbcc.tau[i] = 0.;
}
ForwardDynamicsConstraintsDirect(dbcc.model,dbcc.q,dbcc.qd,
dbcc.tau,dbcc.cs,dbcc.qdd);
ForwardDynamicsConstraintsDirect(dba.model, dba.q, dba.qd,
dba.tau, dba.cs, dba.qdd);
for(unsigned int i = 0; i < dba.cs.G.rows(); ++i){
for(unsigned int j=0; j< dba.cs.G.cols();++j){
CHECK_CLOSE(dba.cs.G(i,j),dbcc.cs.G(i,j),TEST_PREC);
}
}
for(unsigned int i = 0; i < dba.cs.gamma.rows(); ++i){
CHECK_CLOSE(dba.cs.gamma[i],dbcc.cs.gamma[i],TEST_PREC);
}
for(unsigned int i =0; i < dba.cs.constraintAxis.size(); ++i){
for(unsigned int j=0; j< dba.cs.constraintAxis[0].rows();++j){
CHECK_CLOSE(dba.cs.constraintAxis[i][j],
dbcc.cs.constraintAxis[i][j],TEST_PREC);
}
}
SpatialVector a010c =
CalcPointAcceleration6D(dbcc.model,dbcc.q,dbcc.qd,dbcc.qdd,
dbcc.idB1,Vector3d(0.,0.,0.),true);
SpatialVector a020c =
CalcPointAcceleration6D(dbcc.model,dbcc.q,dbcc.qd,dbcc.qdd,
dbcc.idB2,Vector3d(0.,0.,0.),true);
SpatialVector a030c =
CalcPointAcceleration6D(dbcc.model,dbcc.q,dbcc.qd,dbcc.qdd,
dbcc.idB2,Vector3d(dbcc.l2,0.,0.),true);
for(unsigned int i=0; i<6;++i){
CHECK_CLOSE(a010[i],a010c[i],TEST_PREC);
CHECK_CLOSE(a020[i],a020c[i],TEST_PREC);
CHECK_CLOSE(a030[i],a030c[i],TEST_PREC);
}
}