Human36Model.cc

Go to the documentation of this file.

#include "rdl_benchmark/Human36Model.h"
#include "rdl_dynamics/Model.h"

using namespace RobotDynamics;
using namespace RobotDynamics::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)
{
    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 RobotDynamics::Body(SegmentMass[segment], RobotDynamics::Math::Vector3d(SegmentCOM[segment][0], SegmentCOM[segment][1], SegmentCOM[segment][2]), inertia_C);
}

void generate_human36model(RobotDynamics::ModelPtr 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_yxz(SpatialVector(0., 1., 0., 0., 0., 0.), SpatialVector(1., 0., 0., 0., 0., 0.), SpatialVector(0., 0., 1., 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 = SpatialVector(0., 0., 0., 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_yxz, 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_yxz, 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_yxz, 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_yxz, 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_yxz, 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_yxz, upperarm_body, "head");
}