462 lines
21 KiB
C
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 (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
|