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

860 lines
26 KiB
C++

/*
* RBDL - Rigid Body Dynamics Library
* Copyright (c) 2016-2018 Matthew Millard <matthew.millard@iwr.uni-heidelberg.de>
*/
#include <UnitTest++.h>
#include <iostream>
#include "Fixtures.h"
#include "Human36Fixture.h"
#include "rbdl/rbdl_mathutils.h"
#include "rbdl/Logging.h"
#include "rbdl/Model.h"
#include "rbdl/Kinematics.h"
#include "rbdl/Dynamics.h"
#include "rbdl/Constraints.h"
#include <vector>
using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
const double TEST_PREC = 2.0e-12;
const int NUMBER_OF_MODELS = 2;
//==============================================================================
/*
The purpose of this test is to test that all of the code in RBDL
related to a single CustomJoint functions. To do this we will implement
joints that already exist in RBDL but using the CustomJoint interface. The
test will then numerically compare the results produced by the CustomJoint
and the equivalent built-in joint in RBDL. The following algorithms will
be tested:
UpdateKinematicsCustom
Jacobians
InverseDynamics
CompositeRigidBodyAlgorithm
ForwardDynamics
CalcMInvTimestau
ForwardDynamicsContactsKokkevis
*/
//==============================================================================
//==============================================================================
/*
As a note, below are the basic fields and functions that every CustomJoint
class member must provide. Refer to Featherstone's dynamics text if the field
names below don't make sense to you.
1. Extend from CustomJoint:
struct CustomJointClass: public CustomJoint
2. Make a default constructor, and initialize member variables
mDoFCount
S
d_u
e.g.
CustomJointClass()
3. Implement the method jcalc. This method must populate X_lambda, v_J, c_J,
and S.
virtual void jcalc
model.X_lambda[joint_id]
model.v_J
model.c_J
model.mCustomJoints[joint.custom_joint_index]->S = S
4. Implement the method jcalc_X_lambda_S. This method must populate X_lambda
and S.
virtual void jcalc_X_lambda_S
model.X_lambda
model.mCustomJoints[joint.custom_joint_index]->S = S;
*/
//==============================================================================
//Custom Joint Code
//==============================================================================
struct CustomJointTypeRevoluteX : public CustomJoint
{
CustomJointTypeRevoluteX(){
mDoFCount = 1;
S = MatrixNd::Zero(6,1);
S(0, 0) = 1.0;
d_u = MatrixNd::Zero(mDoFCount,1);
}
virtual void jcalc (Model &model,
unsigned int joint_id,
const Math::VectorNd &q,
const Math::VectorNd &qdot)
{
model.X_lambda[joint_id] = Xrotx(q[model.mJoints[joint_id].q_index])
* model.X_T[joint_id];
model.v_J[joint_id][0] = qdot[model.mJoints[joint_id].q_index];
}
virtual void jcalc_X_lambda_S ( Model &model,
unsigned int joint_id,
const Math::VectorNd &q)
{
model.X_lambda[joint_id] =
Xrotx (q[model.mJoints[joint_id].q_index])
* model.X_T[joint_id];
const Joint &joint = model.mJoints[joint_id];
model.mCustomJoints[joint.custom_joint_index]->S = S;
}
};
struct CustomEulerZYXJoint : public CustomJoint
{
CustomEulerZYXJoint ()
{
mDoFCount = 3;
S = MatrixNd::Zero (6,3);
d_u = MatrixNd::Zero(mDoFCount,1);
}
virtual void jcalc (Model &model,
unsigned int joint_id,
const Math::VectorNd &q,
const Math::VectorNd &qdot)
{
double q0 = q[model.mJoints[joint_id].q_index];
double q1 = q[model.mJoints[joint_id].q_index + 1];
double q2 = q[model.mJoints[joint_id].q_index + 2];
double s0 = sin (q0);
double c0 = cos (q0);
double s1 = sin (q1);
double c1 = cos (q1);
double s2 = sin (q2);
double c2 = cos (q2);
SpatialTransform X_J (Matrix3d(
c0 * c1, s0 * c1, -s1,
c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2
),
Vector3d::Zero());
model.X_lambda[joint_id] = X_J * model.X_T[joint_id];
S.setZero();
S(0,0) = -s1;
S(0,2) = 1.;
S(1,0) = c1 * s2;
S(1,1) = c2;
S(2,0) = c1 * c2;
S(2,1) = - s2;
double qdot0 = qdot[model.mJoints[joint_id].q_index];
double qdot1 = qdot[model.mJoints[joint_id].q_index + 1];
double qdot2 = qdot[model.mJoints[joint_id].q_index + 2];
model.v_J[joint_id] = S * Vector3d (qdot0, qdot1, qdot2);
model.c_J[joint_id].set(
-c1*qdot0*qdot1,
-s1*s2*qdot0*qdot1 + c1*c2*qdot0*qdot2 - s2*qdot1*qdot2,
-s1*c2*qdot0*qdot1 - c1*s2*qdot0*qdot2 - c2*qdot1*qdot2,
0., 0., 0.
);
}
virtual void jcalc_X_lambda_S ( Model &model,
unsigned int joint_id,
const Math::VectorNd &q)
{
double q0 = q[model.mJoints[joint_id].q_index];
double q1 = q[model.mJoints[joint_id].q_index + 1];
double q2 = q[model.mJoints[joint_id].q_index + 2];
double s0 = sin (q0);
double c0 = cos (q0);
double s1 = sin (q1);
double c1 = cos (q1);
double s2 = sin (q2);
double c2 = cos (q2);
model.X_lambda[joint_id] = SpatialTransform (
Matrix3d(
c0 * c1, s0 * c1, -s1,
c0 * s1 * s2 - s0 * c2, s0 * s1 * s2 + c0 * c2, c1 * s2,
c0 * s1 * c2 + s0 * s2, s0 * s1 * c2 - c0 * s2, c1 * c2
),
Vector3d (0., 0., 0.)) * model.X_T[joint_id];
S.setZero();
S(0,0) = -s1;
S(0,2) = 1.;
S(1,0) = c1 * s2;
S(1,1) = c2;
S(2,0) = c1 * c2;
S(2,1) = - s2;
const Joint &joint = model.mJoints[joint_id];
model.mCustomJoints[joint.custom_joint_index]->S = S;
//assert (false && "Not yet implemented!");
}
};
//==============================================================================
//Test Fixture
//==============================================================================
struct CustomJointSingleBodyFixture {
CustomJointSingleBodyFixture () {
reference_model.resize(NUMBER_OF_MODELS);
custom_model.resize(NUMBER_OF_MODELS);
body.resize(NUMBER_OF_MODELS);
custom_joint.resize(NUMBER_OF_MODELS);
reference_body_id.resize(NUMBER_OF_MODELS);
custom_body_id.resize(NUMBER_OF_MODELS);
q.resize(NUMBER_OF_MODELS);
qdot.resize(NUMBER_OF_MODELS);
qddot.resize(NUMBER_OF_MODELS);
tau.resize(NUMBER_OF_MODELS);
//========================================================
//Test Model 0: 3dof rotational custom joint vs. standard.
//========================================================
custom_joint0 = CustomEulerZYXJoint();
Matrix3d inertia0 = Matrix3d::Identity(3,3);
Body body0 = Body (1., Vector3d (1.1, 1.2, 1.3), inertia0);
Model reference0, custom0;
unsigned int reference_body_id0 =
reference0.AddBody ( 0,
SpatialTransform(),
Joint(JointTypeEulerZYX),
body0);
unsigned int custom_body_id0 =
custom0.AddBodyCustomJoint ( 0,
SpatialTransform(),
&custom_joint0,
body0);
VectorNd q0 = VectorNd::Zero (reference0.q_size);
VectorNd qdot0 = VectorNd::Zero (reference0.qdot_size);
VectorNd qddot0 = VectorNd::Zero (reference0.qdot_size);
VectorNd tau0 = VectorNd::Zero (reference0.qdot_size);
reference_model.at(0) = reference0;
custom_model.at(0) = custom0;
reference_body_id.at(0) = (reference_body_id0);
custom_body_id.at(0) = (custom_body_id0);
body.at(0) = (body0);
custom_joint.at(0) = (&custom_joint0);
q.at(0) = (q0);
qdot.at(0) = (qdot0);
qddot.at(0) = (qddot0);
tau.at(0) = (tau0);
//========================================================
//Test Model 1: 1 dof rotational custom joint vs. standard.
//========================================================
custom_joint1 = CustomJointTypeRevoluteX();
Model reference1, custom1;
unsigned int reference_body_id1 =
reference1.AddBody ( 0,
SpatialTransform(),
Joint(JointTypeRevoluteX),
body0);
unsigned int custom_body_id1 =
custom1.AddBodyCustomJoint (0,
SpatialTransform(),
&custom_joint1,
body0);
VectorNd q1 = VectorNd::Zero (reference1.q_size);
VectorNd qdot1 = VectorNd::Zero (reference1.qdot_size);
VectorNd qddot1 = VectorNd::Zero (reference1.qdot_size);
VectorNd tau1 = VectorNd::Zero (reference1.qdot_size);
reference_model.at(1) = (reference1);
custom_model.at(1) = (custom1);
reference_body_id.at(1) = (reference_body_id1);
custom_body_id.at(1) = (custom_body_id1);
body.at(1) = (body0);
custom_joint.at(1) = (&custom_joint1);
q.at(1) = (q1);
qdot.at(1) = (qdot1);
qddot.at(1) = (qddot1);
tau.at(1) = (tau1);
}
/*
~CustomJointSingleBodyFixture () {
delete reference_model;
delete custom_model;
delete body;
delete custom_joint;
delete reference_body_id;
delete custom_body_id;
delete q;
delete qdot;
delete qddot;
delete tau;
}*/
vector < Model > reference_model;
vector < Model > custom_model;
vector < Body > body;
vector < CustomJoint* > custom_joint;
vector < unsigned int > reference_body_id;
vector < unsigned int > custom_body_id;
vector < VectorNd > q;
vector < VectorNd > qdot;
vector < VectorNd > qddot;
vector < VectorNd > tau;
CustomJointTypeRevoluteX custom_joint1;
CustomEulerZYXJoint custom_joint0;
};
//==============================================================================
//
// Tests
// UpdateKinematicsCustom
// Jacobians
// InverseDynamics
// CompositeRigidBodyAlgorithm
// ForwardDynamics
// CalcMInvTimestau
// ForwardDynamicsContactsKokkevis
//
//==============================================================================
TEST_FIXTURE ( CustomJointSingleBodyFixture, UpdateKinematics ) {
VectorNd test;
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
unsigned int dof = reference_model.at(idx).dof_count;
for (unsigned int i = 0; i < dof ; i++) {
q.at(idx)[i] = i * 0.1;
qdot.at(idx)[i] = i * 0.15;
qddot.at(idx)[i] = i * 0.17;
}
UpdateKinematics (reference_model.at(idx),
q.at(idx),
qdot.at(idx),
qddot.at(idx));
UpdateKinematics (custom_model.at(idx),
q.at(idx),
qdot.at(idx),
qddot.at(idx));
CHECK_ARRAY_CLOSE (
reference_model.at(idx).X_base[reference_body_id.at(idx)].E.data(),
custom_model.at(idx).X_base[ custom_body_id.at(idx)].E.data(),
9,
TEST_PREC);
CHECK_ARRAY_CLOSE (
reference_model.at(idx).v[reference_body_id.at(idx)].data(),
custom_model.at(idx).v[ custom_body_id.at(idx)].data(),
6,
TEST_PREC);
CHECK_ARRAY_CLOSE (
reference_model.at(idx).a[reference_body_id.at(idx)].data(),
custom_model.at(idx).a[ custom_body_id.at(idx)].data(),
6,
TEST_PREC);
}
}
TEST_FIXTURE (CustomJointSingleBodyFixture, UpdateKinematicsCustom) {
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
unsigned int dof = reference_model.at(idx).dof_count;
for (unsigned int i = 0; i < dof; i++) {
q.at(idx)[i] = i * 9.133758561390194e-01;
qdot.at(idx)[i] = i * 6.323592462254095e-01;
qddot.at(idx)[i] = i * 9.754040499940952e-02;
}
UpdateKinematicsCustom (reference_model.at(idx),
&q.at(idx), NULL, NULL);
UpdateKinematicsCustom (custom_model.at(idx),
&q.at(idx), NULL, NULL);
CHECK_ARRAY_CLOSE (
reference_model.at(idx).X_base[reference_body_id.at(idx)].E.data(),
custom_model.at(idx).X_base[ custom_body_id.at(idx)].E.data(),
9,
TEST_PREC);
//velocity
UpdateKinematicsCustom (reference_model.at(idx),
&q.at(idx),
&qdot.at(idx),
NULL);
UpdateKinematicsCustom (custom_model.at(idx),
&q.at(idx),
&qdot.at(idx),
NULL);
CHECK_ARRAY_CLOSE (
reference_model.at(idx).v[reference_body_id.at(idx)].data(),
custom_model.at(idx).v[ custom_body_id.at(idx)].data(),
6,
TEST_PREC);
//All
UpdateKinematicsCustom (reference_model.at(idx),
&q.at(idx),
&qdot.at(idx),
&qddot.at(idx));
UpdateKinematicsCustom (custom_model.at(idx),
&q.at(idx),
&qdot.at(idx),
&qddot.at(idx));
CHECK_ARRAY_CLOSE (
reference_model.at(idx).a[reference_body_id.at(idx)].data(),
custom_model.at(idx).a[ custom_body_id.at(idx)].data(),
6,
TEST_PREC);
}
}
TEST_FIXTURE (CustomJointSingleBodyFixture, Jacobians) {
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
unsigned int dof = reference_model.at(idx).dof_count;
for (unsigned int i = 0; i < dof; i++) {
q.at(idx)[i] = i * 9.133758561390194e-01;
qdot.at(idx)[i] = i * 6.323592462254095e-01;
qddot.at(idx)[i] = i * 9.754040499940952e-02;
}
//position
UpdateKinematics (reference_model.at(idx),
q.at(idx),
qdot.at(idx),
qddot.at(idx));
UpdateKinematics (custom_model.at(idx),
q.at(idx),
qdot.at(idx),
qddot.at(idx));
//Check the Spatial Jacobian
MatrixNd Gref = MatrixNd(
MatrixNd::Zero(6,reference_model.at(idx).dof_count));
MatrixNd Gcus = MatrixNd(
MatrixNd::Zero(6,reference_model.at(idx).dof_count));
CalcBodySpatialJacobian ( reference_model.at(idx),
q.at(idx),
reference_body_id.at(idx),
Gref);
CalcBodySpatialJacobian ( custom_model.at(idx),
q.at(idx),
custom_body_id.at(idx),
Gcus);
for(int i=0; i<6;++i){
for(unsigned int j = 0; j < dof; ++j){
CHECK_CLOSE (
Gref(i,j),
Gcus(i,j),
TEST_PREC);
}
}
//Check the 6d point Jacobian
Vector3d point_position (1.1, 1.2, 2.1);
CalcPointJacobian6D (reference_model.at(idx),
q.at(idx),
reference_body_id.at(idx),
point_position,
Gref);
CalcPointJacobian6D (custom_model.at(idx),
q.at(idx),
custom_body_id.at(idx),
point_position,
Gcus);
for(int i=0; i<6;++i){
for(unsigned int j = 0; j < dof; ++j){
CHECK_CLOSE (
Gref(i,j),
Gcus(i,j),
TEST_PREC);
}
}
//Check the 3d point Jacobian
MatrixNd GrefPt = MatrixNd::Constant (3,
reference_model.at(idx).dof_count,
0.);
MatrixNd GcusPt = MatrixNd::Constant (3,
reference_model.at(idx).dof_count,
0.);
CalcPointJacobian (reference_model.at(idx),
q.at(idx),
reference_body_id.at(idx),
point_position,
GrefPt);
CalcPointJacobian (custom_model.at(idx),
q.at(idx),
custom_body_id.at(idx),
point_position,
GcusPt);
for(int i=0; i<3;++i){
for(unsigned int j = 0; j < dof; ++j){
CHECK_CLOSE (
GrefPt(i,j),
GcusPt(i,j),
TEST_PREC);
}
}
}
}
TEST_FIXTURE (CustomJointSingleBodyFixture, InverseDynamics) {
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
unsigned int dof = reference_model.at(idx).dof_count;
for (unsigned int i = 0; i < dof; i++) {
q.at(idx)[i] = i * 9.133758561390194e-01;
qdot.at(idx)[i] = i * 6.323592462254095e-01;
qddot.at(idx)[i] = i * 9.754040499940952e-02;
}
//position
VectorNd tauRef = VectorNd::Zero (reference_model.at(idx).qdot_size);
VectorNd tauCus = VectorNd::Zero (reference_model.at(idx).qdot_size);
InverseDynamics(reference_model.at(idx),
q.at(idx),
qdot.at(idx),
qddot.at(idx),
tauRef);
InverseDynamics(custom_model.at(idx),
q.at(idx),
qdot.at(idx),
qddot.at(idx),
tauCus);
VectorNd tauErr = tauRef-tauCus;
CHECK_ARRAY_CLOSE (
tauRef.data(),
tauCus.data(),
tauRef.rows(),
TEST_PREC);
}
}
TEST_FIXTURE (CustomJointSingleBodyFixture, CompositeRigidBodyAlgorithm) {
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
unsigned int dof = reference_model.at(idx).dof_count;
for (unsigned int i = 0; i < dof; i++) {
q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01;
qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01;
tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02;
}
MatrixNd h_ref = MatrixNd::Constant (dof, dof, 0.);
VectorNd c_ref = VectorNd::Constant (dof, 0.);
VectorNd qddot_zero_ref = VectorNd::Constant (dof, 0.);
VectorNd qddot_crba_ref = VectorNd::Constant (dof, 0.);
MatrixNd h_cus = MatrixNd::Constant (dof, dof, 0.);
VectorNd c_cus = VectorNd::Constant (dof, 0.);
VectorNd qddot_zero_cus = VectorNd::Constant (dof, 0.);
VectorNd qddot_crba_cus = VectorNd::Constant (dof, 0.);
VectorNd qddotRef = VectorNd::Zero (dof);
VectorNd qddotCus = VectorNd::Zero (dof);
//Ref
ForwardDynamics(reference_model.at(idx),
q.at(idx),
qdot.at(idx),
tau.at(idx),
qddotRef);
CompositeRigidBodyAlgorithm (reference_model.at(idx),
q.at(idx),
h_ref);
InverseDynamics (reference_model.at(idx),
q.at(idx),
qdot.at(idx),
qddot_zero_ref,
c_ref);
LinSolveGaussElimPivot (h_ref,
c_ref * -1. + tau.at(idx),
qddot_crba_ref);
//Custom
ForwardDynamics(custom_model.at(idx),
q.at(idx),
qdot.at(idx),
tau.at(idx),
qddotCus);
CompositeRigidBodyAlgorithm (custom_model.at(idx),
q.at(idx),
h_cus);
InverseDynamics (custom_model.at(idx),
q.at(idx),
qdot.at(idx),
qddot_zero_cus,
c_cus);
LinSolveGaussElimPivot (h_cus,
c_cus * -1. + tau.at(idx),
qddot_crba_cus);
CHECK_ARRAY_CLOSE(qddot_crba_ref.data(),
qddot_crba_cus.data(),
dof,
TEST_PREC);
}
}
TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamics) {
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
unsigned int dof = reference_model.at(idx).dof_count;
for (unsigned int i = 0; i < dof; i++) {
q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01;
qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01;
qddot.at(idx)[i] = (i+0.1) * 2.323592499940952e-01;
tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02;
}
VectorNd qddotRef = VectorNd::Zero (reference_model.at(idx).qdot_size);
VectorNd qddotCus = VectorNd::Zero (reference_model.at(idx).qdot_size);
ForwardDynamics(reference_model.at(idx),
q.at(idx),
qdot.at(idx),
tau.at(idx),
qddotRef);
ForwardDynamics(custom_model.at(idx),
q.at(idx),
qdot.at(idx),
tau.at(idx),
qddotCus);
CHECK_ARRAY_CLOSE ( qddotRef.data(),
qddotCus.data(),
dof,
TEST_PREC);
}
}
TEST_FIXTURE (CustomJointSingleBodyFixture, CalcMInvTimestau) {
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
unsigned int dof = reference_model.at(idx).dof_count;
for (unsigned int i = 0; i < dof; i++) {
q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01;
tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02;
}
//reference
VectorNd qddot_minv_ref = VectorNd::Zero(dof);
CalcMInvTimesTau(reference_model.at(idx),
q.at(idx),
tau.at(idx),
qddot_minv_ref,
true);
//custom
VectorNd qddot_minv_cus = VectorNd::Zero(dof);
CalcMInvTimesTau(custom_model.at(idx),
q.at(idx),
tau.at(idx),
qddot_minv_cus,
true);
//check.
CHECK_ARRAY_CLOSE(qddot_minv_ref.data(),
qddot_minv_cus.data(),
dof,
TEST_PREC);
}
}
TEST_FIXTURE (CustomJointSingleBodyFixture, ForwardDynamicsContactsKokkevis){
for(int idx =0; idx < NUMBER_OF_MODELS; ++idx){
unsigned int dof = reference_model.at(idx).dof_count;
//Adding a 1 constraint to a system with 1 dof is
//a no-no
if(dof > 1){
for (unsigned int i = 0; i < dof; i++) {
q.at(idx)[i] = (i+0.1) * 9.133758561390194e-01;
qdot.at(idx)[i] = (i+0.1) * 6.323592462254095e-01;
tau.at(idx)[i] = (i+0.1) * 9.754040499940952e-02;
}
VectorNd qddot_ref = VectorNd::Zero(dof);
VectorNd qddot_cus = VectorNd::Zero(dof);
VectorNd qdot_plus_ref = VectorNd::Zero(dof);
VectorNd qdot_plus_cus = VectorNd::Zero(dof);
Vector3d contact_point ( 0., 1., 0.);
ConstraintSet constraint_set_ref;
ConstraintSet constraint_set_cus;
//Reference
constraint_set_ref.AddContactConstraint( reference_body_id.at(idx),
contact_point,
Vector3d (1., 0., 0.),
"ground_x");
constraint_set_ref.AddContactConstraint( reference_body_id.at(idx),
contact_point,
Vector3d (0., 1., 0.),
"ground_y");
constraint_set_ref.Bind (reference_model.at(idx));
//Custom
constraint_set_cus.AddContactConstraint( custom_body_id.at(idx),
contact_point,
Vector3d (1., 0., 0.),
"ground_x");
constraint_set_cus.AddContactConstraint( custom_body_id.at(idx),
contact_point,
Vector3d (0., 1., 0.),
"ground_y");
constraint_set_cus.Bind (custom_model.at(idx));
ComputeConstraintImpulsesDirect(reference_model.at(idx),
q.at(idx),
qdot.at(idx),
constraint_set_ref,
qdot_plus_ref);
ForwardDynamicsContactsKokkevis (reference_model.at(idx),
q.at(idx),
qdot_plus_ref,
tau.at(idx),
constraint_set_ref,
qddot_ref);
ComputeConstraintImpulsesDirect(custom_model.at(idx),
q.at(idx),
qdot.at(idx),
constraint_set_cus,
qdot_plus_cus);
ForwardDynamicsContactsKokkevis (custom_model.at(idx),
q.at(idx),
qdot_plus_cus,
tau.at(idx),
constraint_set_cus,
qddot_cus);
VectorNd qdot_plus_error = qdot_plus_ref - qdot_plus_cus;
VectorNd qddot_error = qddot_ref - qddot_cus;
CHECK_ARRAY_CLOSE (qdot_plus_ref.data(),
qdot_plus_cus.data(),
dof,
TEST_PREC);
CHECK_ARRAY_CLOSE (qddot_ref.data(),
qddot_cus.data(),
dof,
TEST_PREC);
}
}
}