rbdl / addons / benchmark / Human36Model.cc

#include "Human36Model.h"

#include "rbdl/rbdl.h"

using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;

enum SegmentName {
	SegmentPelvis = 0,
	SegmentThigh,
	SegmentShank,
	SegmentFoot,
	SegmentMiddleTrunk,
	SegmentUpperTrunk,
	SegmentUpperArm,
	SegmentLowerArm,
	SegmentHand,
	SegmentHead,
	SegmentNameLast
};

double SegmentLengths[SegmentNameLast] = {
	0.1457,
	0.4222,
	0.4403,
	0.1037,
	0.2155,
	0.2421,
	0.2817,
	0.2689,
	0.0862,
	0.2429
};

double SegmentMass[SegmentNameLast] = {
	0.8154,
	10.3368,
	3.1609,
	1.001,
	16.33,
	15.96,
	1.9783,
	1.1826,
	0.4453,
	5.0662
};

double SegmentCOM[SegmentNameLast][3] = {
	{ 0., 0.,  0.0891},
	{ 0., 0., -0.1729},
	{ 0., 0., -0.1963},
	{ 0.1254, 0., -0.0516},
	{ 0., 0.,  0.1185},
	{ 0., 0.,  0.1195},
	{ 0., 0., -0.1626},
	{ 0., 0., -0.1230},
	{ 0., 0., -0.0680},
	{ 0., 0.,  1.1214}
};

double SegmentRadiiOfGyration[SegmentNameLast][3] = {
	{ 0.0897, 0.0855, 0.0803},
	{ 0.1389, 0.0629, 0.1389},
	{ 0.1123, 0.0454, 0.1096},
	{ 0.0267, 0.0129, 0.0254},
	{ 0.0970, 0.1009, 0.0825},
 	{ 0.1273, 0.1172, 0.0807},
	{ 0.0803, 0.0758, 0.0445},
	{ 0.0742, 0.0713, 0.0325},
	{ 0.0541, 0.0442, 0.0346},
	{ 0.0736, 0.0634, 0.0765}
};

Body create_body (SegmentName segment) {
	return Body (
			SegmentMass[segment],
			Vector3d (
				SegmentCOM[segment][0],
				SegmentCOM[segment][1],
				SegmentCOM[segment][2]
				),
			Vector3d (
				SegmentRadiiOfGyration[segment][0],
				SegmentRadiiOfGyration[segment][1],
				SegmentRadiiOfGyration[segment][2]
				)
			);
}

void generate_human36model (RigidBodyDynamics::Model *model) {
	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);

	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 (0., 0., 1., 0., 0., 0.),
		SpatialVector (1., 0., 0., 0., 0., 0.)
		);

	Joint rot_yzx (
			SpatialVector (0., 1., 0., 0., 0., 0.),
			SpatialVector (0., 0., 1., 0., 0., 0.),
			SpatialVector (1., 0., 0., 0., 0., 0.)
			);

	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);

	model->gravity = Vector3d (0., 0., -9.81);

	unsigned int pelvis_id = model->AddBody (0, Xtrans (Vector3d (0., 0., 0.)), free_flyer, pelvis_body, "pelvis");

	// right leg
	model->AddBody (pelvis_id, Xtrans(Vector3d(0., -0.0872, 0.)), rot_yzx, thigh_body, "thigh_r");
	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_r");
	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_r");

	// left leg
	model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0.0872, 0.)), rot_yzx, thigh_body, "thigh_l");
	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentThigh])), rot_y, shank_body, "shank_l");
	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentShank])), rot_yz, foot_body, "foot_l");

	// trunk
	model->AddBody (pelvis_id, Xtrans(Vector3d(0., 0., SegmentLengths[SegmentPelvis])), rot_yzx, middle_trunk_body, "middletrunk");
	unsigned int uppertrunk_id = model->AppendBody (Xtrans(Vector3d(0., 0., SegmentLengths[SegmentMiddleTrunk])), fixed, upper_trunk_body, "uppertrunk");

	// right arm
	model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., -0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yzx, upperarm_body, "upperarm_r");
	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_r");
	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_r");

	// left arm
	model->AddBody (uppertrunk_id, Xtrans(Vector3d(0.,  0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yzx, upperarm_body, "upperarm_l");
	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentUpperArm])), rot_y, lowerarm_body, "lowerarm_l");
	model->AppendBody (Xtrans(Vector3d(0., 0., -SegmentLengths[SegmentLowerArm])), rot_yz, hand_body, "hand_l");

	// head	
	model->AddBody (uppertrunk_id, Xtrans(Vector3d(0., 0.1900, SegmentLengths[SegmentUpperTrunk])), rot_yzx, upperarm_body, "head");
}
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.