martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
rbdlsim/3rdparty/rbdl/tests/ForwardDynamicsConstraintsE...

222 lines
7.7 KiB
C++
Raw Blame History

#include "rbdl_tests.h"
#include "rbdl/rbdl.h"
#include <cassert>
#include "PendulumModels.h"
using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
const double TEST_PREC = 1.0e-11;
// Reduce an angle to the (-pi, pi] range.
static double inRange(double angle) {
while(angle > M_PI) {
angle -= 2. * M_PI;
}
while(angle <= -M_PI) {
angle += 2. * M_PI;
}
return angle;
}
TEST_CASE (__FILE__"_ForwardDynamicsConstraintsWithExternalForcesCorrectnessTest", "") {
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.;
std::vector < SpatialVector > fext_dbj,fext_dba;
fext_dbj.resize( dbj.model.mBodies.size() );
fext_dba.resize( dba.model.mBodies.size() );
for(unsigned int i=0; i<dbj.model.mBodies.size();++i){
fext_dbj[i]=SpatialVector::Zero();
}
for(unsigned int i=0; i<dba.model.mBodies.size();++i){
fext_dba[i]=SpatialVector::Zero();
}
SpatialVector fextB1 = SpatialVector( 7.922073295595544e+00,
9.594924263929030e+00,
6.557406991565868e+00,
3.571167857418955e-01,
8.491293058687772e+00,
9.339932477575505e+00);
SpatialVector fextB2 = SpatialVector( 6.787351548577734e+00,
7.577401305783335e+00,
7.431324681249162e+00,
3.922270195341682e+00,
6.554778901775567e+00,
1.711866878115618e+00);
fext_dbj[dbj.idB1] = fextB1;
fext_dbj[dbj.idB2] = fextB2;
fext_dba[dba.idB1] = fextB1;
fext_dba[dba.idB2] = fextB2;
for(unsigned int i=0; i<dbj.qdd.size();++i){
dbj.qdd[i]=0.;
}
UpdateKinematics(dbj.model,dbj.q,dbj.qd,dbj.qdd);
ForwardDynamics(dbj.model,dbj.q,dbj.qd,dbj.tau,dbj.qdd,&fext_dbj);
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
/*
dba.q[0] = dbj.q[0];
dba.q[1] = r020[0];
dba.q[2] = r020[1];
dba.q[3] = r020[2];
dba.q[4] = 0;
dba.q[5] = 0;
dba.q[6] = dbj.q[0]+dbj.q[1];
dba.qd[0] = dbj.qd[0];
dba.qd[1] = v020[0];
dba.qd[2] = v020[1];
dba.qd[3] = v020[2];
dba.qd[4] = 0;
dba.qd[5] = 0;
dba.qd[6] = dbj.qd[0]+dbj.qd[1];
*/
dba.q[0] = r010[0];
dba.q[1] = r010[1];
dba.q[2] = r010[2];
dba.q[3] = dbj.q[0];
dba.q[4] = 0;
dba.q[5] = 0;
dba.q[6] = r020[0];
dba.q[7] = r020[1];
dba.q[8] = r020[2];
dba.q[9] = dbj.q[0];
dba.q[10] = dbj.q[1];
dba.q[11] = 0;
dba.qd[0] = v010[3];
dba.qd[1] = v010[4];
dba.qd[2] = v010[5];
dba.qd[3] = dbj.qd[0];
dba.qd[4] = 0;
dba.qd[5] = 0;
dba.qd[6] = v020[3];
dba.qd[7] = v020[4];
dba.qd[8] = v020[5];
dba.qd[9] = dbj.qd[0];
dba.qd[10] = dbj.qd[1];
dba.qd[11] = 0;
VectorNd err(dba.cs.size());
VectorNd errd(dba.cs.size());
CalcConstraintsPositionError(dba.model,dba.q,dba.cs,err,true);
CalcConstraintsVelocityError(dba.model,dba.q,dba.qd,dba.cs,errd,true);
//The constraint errors at the position and velocity level
//must be zero before the accelerations can be tested.
VectorNd target1 = VectorNd::Zero(err.rows());
REQUIRE_THAT (target1, AllCloseVector(err, TEST_PREC, TEST_PREC));
VectorNd target2 = VectorNd::Zero(errd.rows());
REQUIRE_THAT (target2, AllCloseVector(errd, TEST_PREC, TEST_PREC));
//Evaluate the accelerations of the constrained pendulum and
//compare those to the joint-coordinate pendulum
for(unsigned int i=0; i<dba.tau.rows();++i){
dba.tau[i] = 0.;
}
ForwardDynamicsConstraintsDirect(dba.model,dba.q,dba.qd,
dba.tau,dba.cs,dba.qdd,&fext_dba);
SpatialVector a010c =
CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB1,Vector3d(0.,0.,0.),true);
SpatialVector a020c =
CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB2,Vector3d(0.,0.,0.),true);
SpatialVector a030c =
CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB2,Vector3d(dba.l2,0.,0.),true);
REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
ForwardDynamicsConstraintsNullSpace(
dba.model,dba.q,dba.qd,
dba.tau,dba.cs,dba.qdd,&fext_dba);
a010c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB1,Vector3d(0.,0.,0.),true);
a020c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB2,Vector3d(0.,0.,0.),true);
a030c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB2,Vector3d(dba.l2,0.,0.),true);
REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
ForwardDynamicsConstraintsRangeSpaceSparse(
dba.model,dba.q,dba.qd,
dba.tau,dba.cs,dba.qdd, &fext_dba);
a010c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB1,Vector3d(0.,0.,0.),true);
a020c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB2,Vector3d(0.,0.,0.),true);
a030c = CalcPointAcceleration6D(dba.model,dba.q,dba.qd,dba.qdd,
dba.idB2,Vector3d(dba.l2,0.,0.),true);
REQUIRE_THAT (a010, AllCloseVector(a010c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a020, AllCloseVector(a020c, TEST_PREC, TEST_PREC));
REQUIRE_THAT (a030, AllCloseVector(a030c, TEST_PREC, TEST_PREC));
}
Reference in New Issue View Git Blame Copy Permalink