860 lines
26 KiB
C++
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);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|