rbdlsim/3rdparty/rbdl/tests/Human36Fixture.h

462 lines
21 KiB
C++

#ifndef RBDL_HUMAN36_FIXTURE
#define RBDL_HUMAN36_FIXTURE
#include "rbdl/rbdl.h"
struct Human36 {
RigidBodyDynamics::Model *model;
RigidBodyDynamics::Model *model_emulated;
RigidBodyDynamics::Model *model_3dof;
RigidBodyDynamics::Math::VectorNd q;
RigidBodyDynamics::Math::VectorNd qdot;
RigidBodyDynamics::Math::VectorNd qddot;
RigidBodyDynamics::Math::VectorNd tau;
RigidBodyDynamics::Math::VectorNd qddot_emulated;
RigidBodyDynamics::Math::VectorNd qddot_3dof;
RigidBodyDynamics::ConstraintSet constraints_1B1C_emulated;
RigidBodyDynamics::ConstraintSet constraints_1B4C_emulated;
RigidBodyDynamics::ConstraintSet constraints_4B4C_emulated;
RigidBodyDynamics::ConstraintSet constraints_1B1C_3dof;
RigidBodyDynamics::ConstraintSet constraints_1B4C_3dof;
RigidBodyDynamics::ConstraintSet constraints_4B4C_3dof;
enum SegmentName {
SegmentPelvis = 0,
SegmentThigh,
SegmentShank,
SegmentFoot,
SegmentMiddleTrunk,
SegmentUpperTrunk,
SegmentUpperArm,
SegmentLowerArm,
SegmentHand,
SegmentHead,
SegmentNameLast
};
enum BodyName {
BodyPelvis,
BodyThighRight,
BodyShankRight,
BodyFootRight,
BodyThighLeft,
BodyShankLeft,
BodyFootLeft,
BodyMiddleTrunk,
BodyUpperTrunk,
BodyUpperArmRight,
BodyLowerArmRight,
BodyHandRight,
BodyUpperArmLeft,
BodyLowerArmLeft,
BodyHandLeft,
BodyHead,
BodyNameLast
};
enum DofNames {
PelvisTX,
PelvisTY,
PelvisTZ,
PelvisRY,
PelvisRX,
PelvisRZ,
HipRightRY,
HipRightRX,
HipRightRZ,
KneeRightRY,
AnkleRightRY,
AnkleRightRZ,
HipLeftRY,
HipLeftRX,
HipLeftRZ,
KneeLeftRY,
AnkleLeftRY,
AnkleLeftRZ,
LumbarRY,
LumbarRX,
LumbarRZ,
ShoulderRightRY,
ShoulderRightRX,
ShoulderRightRZ,
ElbowRightRY,
WristRightRY,
WristRightRZ,
ShoulderLeftRY,
ShoulderLeftRX,
ShoulderLeftRZ,
ElbowLeftRY,
WristLeftRY,
WristLeftRZ,
NeckRY,
NeckRX,
NeckRZ,
DofNameCount
};
double SegmentLengths[SegmentNameLast];
double SegmentMass[SegmentNameLast];
double SegmentCOM[SegmentNameLast][3];
double SegmentRadiiOfGyration[SegmentNameLast][3];
unsigned int body_id_emulated[BodyNameLast];
unsigned int body_id_3dof[BodyNameLast];
void initParameters () {
SegmentLengths[SegmentPelvis ] = 0.1457;
SegmentLengths[SegmentThigh ] = 0.4222;
SegmentLengths[SegmentShank ] = 0.4403;
SegmentLengths[SegmentFoot ] = 0.1037;
SegmentLengths[SegmentMiddleTrunk] = 0.2155;
SegmentLengths[SegmentUpperTrunk ] = 0.2421;
SegmentLengths[SegmentUpperArm ] = 0.2817;
SegmentLengths[SegmentLowerArm ] = 0.2689;
SegmentLengths[SegmentHand ] = 0.0862;
SegmentLengths[SegmentHead ] = 0.2429;
SegmentMass[SegmentPelvis ] = 0.8154;
SegmentMass[SegmentThigh ] = 10.3368;
SegmentMass[SegmentShank ] = 3.1609;
SegmentMass[SegmentFoot ] = 1.001;
SegmentMass[SegmentMiddleTrunk] = 16.33;
SegmentMass[SegmentUpperTrunk ] = 15.96;
SegmentMass[SegmentUpperArm ] = 1.9783;
SegmentMass[SegmentLowerArm ] = 1.1826;
SegmentMass[SegmentHand ] = 0.4453;
SegmentMass[SegmentHead ] = 5.0662;
SegmentCOM[SegmentPelvis ][0] = 0.;
SegmentCOM[SegmentPelvis ][1] = 0.;
SegmentCOM[SegmentPelvis ][2] = 0.0891;
SegmentCOM[SegmentThigh ][0] = 0.;
SegmentCOM[SegmentThigh ][1] = 0.;
SegmentCOM[SegmentThigh ][2] = -0.1729;
SegmentCOM[SegmentShank ][0] = 0.;
SegmentCOM[SegmentShank ][1] = 0.;
SegmentCOM[SegmentShank ][2] = -0.1963;
SegmentCOM[SegmentFoot ][0] = 0.1254;
SegmentCOM[SegmentFoot ][1] = 0.;
SegmentCOM[SegmentFoot ][2] = -0.0516;
SegmentCOM[SegmentMiddleTrunk][0] = 0.;
SegmentCOM[SegmentMiddleTrunk][1] = 0.;
SegmentCOM[SegmentMiddleTrunk][2] = 0.1185;
SegmentCOM[SegmentUpperTrunk ][0] = 0.;
SegmentCOM[SegmentUpperTrunk ][1] = 0.;
SegmentCOM[SegmentUpperTrunk ][2] = 0.1195;
SegmentCOM[SegmentUpperArm ][0] = 0.;
SegmentCOM[SegmentUpperArm ][1] = 0.;
SegmentCOM[SegmentUpperArm ][2] = -0.1626;
SegmentCOM[SegmentLowerArm ][0] = 0.;
SegmentCOM[SegmentLowerArm ][1] = 0.;
SegmentCOM[SegmentLowerArm ][2] = -0.1230;
SegmentCOM[SegmentHand ][0] = 0.;
SegmentCOM[SegmentHand ][1] = 0.;
SegmentCOM[SegmentHand ][2] = -0.0680;
SegmentCOM[SegmentHead ][0] = 0.;
SegmentCOM[SegmentHead ][1] = 0.;
SegmentCOM[SegmentHead ][2] = 1.1214;
SegmentRadiiOfGyration[SegmentPelvis ][0] = 0.0897;
SegmentRadiiOfGyration[SegmentPelvis ][1] = 0.0855;
SegmentRadiiOfGyration[SegmentPelvis ][2] = 0.0803;
SegmentRadiiOfGyration[SegmentThigh ][0] = 0.1389;
SegmentRadiiOfGyration[SegmentThigh ][1] = 0.0629;
SegmentRadiiOfGyration[SegmentThigh ][2] = 0.1389;
SegmentRadiiOfGyration[SegmentShank ][0] = 0.1123;
SegmentRadiiOfGyration[SegmentShank ][1] = 0.0454;
SegmentRadiiOfGyration[SegmentShank ][2] = 0.1096;
SegmentRadiiOfGyration[SegmentFoot ][0] = 0.0267;
SegmentRadiiOfGyration[SegmentFoot ][1] = 0.0129;
SegmentRadiiOfGyration[SegmentFoot ][2] = 0.0254;
SegmentRadiiOfGyration[SegmentMiddleTrunk][0] = 0.0970;
SegmentRadiiOfGyration[SegmentMiddleTrunk][1] = 0.1009;
SegmentRadiiOfGyration[SegmentMiddleTrunk][2] = 0.0825;
SegmentRadiiOfGyration[SegmentUpperTrunk ][0] = 0.1273;
SegmentRadiiOfGyration[SegmentUpperTrunk ][1] = 0.1172;
SegmentRadiiOfGyration[SegmentUpperTrunk ][2] = 0.0807;
SegmentRadiiOfGyration[SegmentUpperArm ][0] = 0.0803;
SegmentRadiiOfGyration[SegmentUpperArm ][1] = 0.0758;
SegmentRadiiOfGyration[SegmentUpperArm ][2] = 0.0445;
SegmentRadiiOfGyration[SegmentLowerArm ][0] = 0.0742;
SegmentRadiiOfGyration[SegmentLowerArm ][1] = 0.0713;
SegmentRadiiOfGyration[SegmentLowerArm ][2] = 0.0325;
SegmentRadiiOfGyration[SegmentHand ][0] = 0.0541;
SegmentRadiiOfGyration[SegmentHand ][1] = 0.0442;
SegmentRadiiOfGyration[SegmentHand ][2] = 0.0346;
SegmentRadiiOfGyration[SegmentHead ][0] = 0.0736;
SegmentRadiiOfGyration[SegmentHead ][1] = 0.0634;
SegmentRadiiOfGyration[SegmentHead ][2] = 0.0765;
};
RigidBodyDynamics::Body create_body (SegmentName segment) {
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
Matrix3d inertia_C (Matrix3d::Zero());
inertia_C(0,0) = pow(SegmentRadiiOfGyration[segment][0] * SegmentLengths[segment], 2) * SegmentMass[segment];
inertia_C(1,1) = pow(SegmentRadiiOfGyration[segment][1] * SegmentLengths[segment], 2) * SegmentMass[segment];
inertia_C(2,2) = pow(SegmentRadiiOfGyration[segment][2] * SegmentLengths[segment], 2) * SegmentMass[segment];
return RigidBodyDynamics::Body (
SegmentMass[segment],
RigidBodyDynamics::Math::Vector3d (
SegmentCOM[segment][0],
SegmentCOM[segment][1],
SegmentCOM[segment][2]
),
inertia_C);
}
void generate () {
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
Body pelvis_body = create_body (SegmentPelvis);
Body thigh_body = create_body (SegmentThigh);
Body shank_body = create_body (SegmentShank);
Body foot_body = create_body (SegmentFoot);
Body middle_trunk_body = create_body (SegmentMiddleTrunk);
Body upper_trunk_body = create_body (SegmentUpperTrunk);
Body upperarm_body = create_body (SegmentUpperArm);
Body lowerarm_body = create_body (SegmentLowerArm);
Body hand_body = create_body (SegmentHand);
Body head_body = create_body (SegmentHead);
Matrix3d zero_matrix (Matrix3d::Zero(3,3));
Body null_body (0., Vector3d (0., 0., 0.), zero_matrix);
Joint free_flyer (
SpatialVector (0., 0., 0., 1., 0., 0.),
SpatialVector (0., 0., 0., 0., 1., 0.),
SpatialVector (0., 0., 0., 0., 0., 1.),
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (1., 0., 0., 0., 0., 0.),
SpatialVector (0., 0., 1., 0., 0., 0.)
);
Joint rot_yxz_emulated (
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (1., 0., 0., 0., 0., 0.),
SpatialVector (0., 0., 1., 0., 0., 0.)
);
Joint trans_xyz = Joint(JointTypeTranslationXYZ);
Joint rot_yxz_3dof = Joint(JointTypeEulerYXZ);
Joint rot_yz (
SpatialVector (0., 1., 0., 0., 0., 0.),
SpatialVector (0., 0., 1., 0., 0., 0.)
);
Joint rot_y (
SpatialVector (0., 1., 0., 0., 0., 0.)
);
Joint fixed (JointTypeFixed);
// Generate emulated model
model_emulated->gravity = Vector3d (0., 0., -9.81);
body_id_emulated[BodyPelvis] = model_emulated->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis");
// right leg
body_id_emulated[BodyThighRight] = model_emulated->AddBody (body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz_emulated, thigh_body, "thigh_r");
body_id_emulated[BodyShankRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r");
body_id_emulated[BodyFootRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r");
// left leg
body_id_emulated[BodyThighLeft] = model_emulated->AddBody (body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz_emulated, thigh_body, "thigh_l");
body_id_emulated[BodyShankLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l");
body_id_emulated[BodyFootLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l");
// trunk
body_id_emulated[BodyMiddleTrunk] = model_emulated->AddBody (body_id_emulated[BodyPelvis], Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yxz_emulated, middle_trunk_body, "middletrunk");
body_id_emulated[BodyUpperTrunk] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk");
// right arm
body_id_emulated[BodyUpperArmRight] = model_emulated->AddBody (body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_emulated, upperarm_body, "upperarm_r");
body_id_emulated[BodyLowerArmRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r");
body_id_emulated[BodyHandRight] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r");
// left arm
body_id_emulated[BodyUpperArmLeft] = model_emulated->AddBody (body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_emulated, upperarm_body, "upperarm_l");
body_id_emulated[BodyLowerArmLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l");
body_id_emulated[BodyHandLeft] = model_emulated->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l");
// head
body_id_emulated[BodyHead] = model_emulated->AddBody (body_id_emulated[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_emulated, upperarm_body, "head");
// Generate 3dof model
model_3dof->gravity = Vector3d (0., 0., -9.81);
unsigned int pelvis_trans = model_3dof->AddBody (0, Xtrans(Vector3d (0., 0., 0.)), trans_xyz, null_body, "pelvis_trans_xyz");
body_id_3dof[BodyPelvis] = model_3dof->AddBody (pelvis_trans, Xtrans (Vector3d (0., 0., 0.)), rot_yxz_3dof, pelvis_body, "pelvis");
// body_id_3dof[BodyPelvis] = model_3dof->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis");
// right leg
body_id_3dof[BodyThighRight] = model_3dof->AddBody (body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., -0.0872, 0.)), rot_yxz_3dof, thigh_body, "thigh_r");
body_id_3dof[BodyShankRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r");
body_id_3dof[BodyFootRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r");
// left leg
body_id_3dof[BodyThighLeft] = model_3dof->AddBody (body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., 0.0872, 0.)), rot_yxz_3dof, thigh_body, "thigh_l");
body_id_3dof[BodyShankLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l");
body_id_3dof[BodyFootLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l");
// trunk
body_id_3dof[BodyMiddleTrunk] = model_3dof->AddBody (body_id_3dof[BodyPelvis], Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yxz_3dof, middle_trunk_body, "middletrunk");
body_id_3dof[BodyUpperTrunk] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk");
// right arm
body_id_3dof[BodyUpperArmRight] = model_3dof->AddBody (body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "upperarm_r");
body_id_3dof[BodyLowerArmRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r");
body_id_3dof[BodyHandRight] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r");
// left arm
body_id_3dof[BodyUpperArmLeft] = model_3dof->AddBody (body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "upperarm_l");
body_id_3dof[BodyLowerArmLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l");
body_id_3dof[BodyHandLeft] = model_3dof->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l");
// head
body_id_3dof[BodyHead] = model_3dof->AddBody (body_id_3dof[BodyUpperTrunk], Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yxz_3dof, upperarm_body, "head");
}
void initConstraintSets () {
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
unsigned int foot_r_emulated = model_emulated->GetBodyId ("foot_r");
unsigned int foot_l_emulated = model_emulated->GetBodyId ("foot_l");
unsigned int hand_r_emulated = model_emulated->GetBodyId ("hand_r");
unsigned int hand_l_emulated = model_emulated->GetBodyId ("hand_l");
constraints_1B1C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_1B1C_emulated.Bind (*model_emulated);
constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_1B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_1B4C_emulated.Bind (*model_emulated);
constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_4B4C_emulated.AddContactConstraint (foot_r_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_4B4C_emulated.AddContactConstraint (foot_l_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_4B4C_emulated.AddContactConstraint (hand_r_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_4B4C_emulated.AddContactConstraint (hand_l_emulated, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_emulated.Bind (*model);
unsigned int foot_r_3dof = model_3dof->GetBodyId ("foot_r");
unsigned int foot_l_3dof = model_3dof->GetBodyId ("foot_l");
unsigned int hand_r_3dof = model_3dof->GetBodyId ("hand_r");
unsigned int hand_l_3dof = model_3dof->GetBodyId ("hand_l");
constraints_1B1C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_1B1C_3dof.Bind (*model_3dof);
constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_1B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_1B4C_3dof.Bind (*model_3dof);
constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_4B4C_3dof.AddContactConstraint (foot_r_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_4B4C_3dof.AddContactConstraint (foot_l_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_4B4C_3dof.AddContactConstraint (hand_r_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 1., 0.));
constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (0.1, 0., -0.05), Vector3d (0., 0., 1.));
constraints_4B4C_3dof.AddContactConstraint (hand_l_3dof, Vector3d (-0.1, 0., -0.05), Vector3d (1., 0., 0.));
constraints_4B4C_3dof.Bind (*model_3dof);
}
void randomizeStates () {
for (unsigned int i = 0; i < q.size(); i++) {
q[i] = 0.4 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qdot[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
tau[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
qddot[i] = 0.5 * M_PI * static_cast<double>(rand()) / static_cast<double>(RAND_MAX);
}
qddot_emulated = qddot;
qddot_3dof = qddot;
}
Human36 () {
ClearLogOutput();
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
initParameters();
model_emulated = new RigidBodyDynamics::Model();
model_3dof = new RigidBodyDynamics::Model();
model = model_emulated;
generate();
initConstraintSets();
q = VectorNd::Zero (model_emulated->q_size);
qdot = VectorNd::Zero (model_emulated->qdot_size);
qddot = VectorNd::Zero (model_emulated->qdot_size);
tau = VectorNd::Zero (model_emulated->qdot_size);
qddot_emulated = VectorNd::Zero (model_emulated->qdot_size);
qddot_3dof= VectorNd::Zero (model_emulated->qdot_size);
};
~Human36 () {
delete model_emulated;
delete model_3dof;
}
};
#endif