void MakeHexapod(DemoEntityManager* const scene, const dMatrix& location)
{
dFloat mass = 30.0f;
// make the kinematic solver
m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton());
// make the root body
dMatrix baseMatrix(dGetIdentityMatrix());
baseMatrix.m_posit.m_y += 0.35f;
dVector size (1.3f, 0.31f, 0.5f, 0.0f);
NewtonBody* const hexaBody = CreateBox(scene, baseMatrix * location, size, mass, 1.0f);
void* const hexaBodyNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, hexaBody);
int legEffectorCount = 0;
dCustomInverseDynamicsEffector* legEffectors[32];
baseMatrix.m_posit.m_y -= 0.06f;
// make the hexapod six limbs
for (int i = 0; i < 3; i ++) {
dMatrix rightLocation (baseMatrix);
rightLocation.m_posit += rightLocation.m_right.Scale (size.m_z * 0.65f);
rightLocation.m_posit += rightLocation.m_front.Scale (size.m_x * 0.3f - size.m_x * i / 3.0f);
legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, rightLocation * location, mass * 0.1f, 0.3f);
legEffectorCount ++;
dMatrix similarTransform (dGetIdentityMatrix());
similarTransform.m_posit.m_x = rightLocation.m_posit.m_x;
similarTransform.m_posit.m_y = rightLocation.m_posit.m_y;
dMatrix leftLocation (rightLocation * similarTransform.Inverse() * dYawMatrix(dPi) * similarTransform);
legEffectors[legEffectorCount] = AddLeg (scene, hexaBodyNode, leftLocation * location, mass * 0.1f, 0.3f);
legEffectorCount ++;
}
// finalize inverse dynamics solver
NewtonInverseDynamicsEndBuild(m_kinematicSolver);
// create a fix pose frame generator
dEffectorTreeFixPose* const idlePose = new dEffectorTreeFixPose(hexaBody);
dEffectorTreeFixPose* const walkPoseGenerator = new dEffectorWalkPoseGenerator(hexaBody);
m_walkIdleBlender = new dEffectorBlendIdleWalk (hexaBody, idlePose, walkPoseGenerator);
m_postureModifier = new dAnimationHipController(m_walkIdleBlender);
m_animTreeNode = new dEffectorTreeRoot(hexaBody, m_postureModifier);
dMatrix rootMatrix;
NewtonBodyGetMatrix (hexaBody, &rootMatrix[0][0]);
rootMatrix = rootMatrix.Inverse();
for (int i = 0; i < legEffectorCount; i++) {
dEffectorTreeInterface::dEffectorTransform frame;
dCustomInverseDynamicsEffector* const effector = legEffectors[i];
dMatrix effectorMatrix(effector->GetBodyMatrix());
dMatrix poseMatrix(effectorMatrix * rootMatrix);
frame.m_effector = effector;
frame.m_posit = poseMatrix.m_posit;
frame.m_rotation = dQuaternion(poseMatrix);
idlePose->GetPose().Append(frame);
walkPoseGenerator->GetPose().Append(frame);
m_animTreeNode->GetPose().Append(frame);
}
}
void MakeKukaRobot(DemoEntityManager* const scene, DemoEntity* const model)
{
m_kinematicSolver = NewtonCreateInverseDynamics(scene->GetNewton());
NewtonBody* const baseFrameBody = CreateBodyPart(model, armRobotConfig[0]);
void* const baseFrameNode = NewtonInverseDynamicsAddRoot(m_kinematicSolver, baseFrameBody);
NewtonBodySetMassMatrix(baseFrameBody, 0.0f, 0.0f, 0.0f, 0.0f);
dMatrix boneAligmentMatrix(
dVector(0.0f, 1.0f, 0.0f, 0.0f),
dVector(0.0f, 0.0f, 1.0f, 0.0f),
dVector(1.0f, 0.0f, 0.0f, 0.0f),
dVector(0.0f, 0.0f, 0.0f, 1.0f));
int stackIndex = 0;
DemoEntity* childEntities[32];
void* parentBones[32];
for (DemoEntity* child = model->GetChild(); child; child = child->GetSibling()) {
parentBones[stackIndex] = baseFrameNode;
childEntities[stackIndex] = child;
stackIndex++;
}
dKukaEffector* effector = NULL;
const int partCount = sizeof(armRobotConfig) / sizeof(armRobotConfig[0]);
while (stackIndex) {
stackIndex--;
DemoEntity* const entity = childEntities[stackIndex];
void* const parentJoint = parentBones[stackIndex];
const char* const name = entity->GetName().GetStr();
for (int i = 0; i < partCount; i++) {
if (!strcmp(armRobotConfig[i].m_partName, name)) {
if (strstr(name, "bone")) {
// add a bone and all it children
dMatrix matrix;
NewtonBody* const limbBody = CreateBodyPart(entity, armRobotConfig[i]);
NewtonBodyGetMatrix(limbBody, &matrix[0][0]);
NewtonBody* const parentBody = NewtonInverseDynamicsGetBody(m_kinematicSolver, parentJoint);
dCustomInverseDynamics* const rotatingColumnHinge = new dCustomInverseDynamics(boneAligmentMatrix * matrix, limbBody, parentBody);
rotatingColumnHinge->SetJointTorque(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f);
rotatingColumnHinge->SetTwistAngle(armRobotConfig[i].m_minLimit * dDegreeToRad, armRobotConfig[i].m_maxLimit * dDegreeToRad);
void* const limbJoint = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, parentJoint, rotatingColumnHinge->GetJoint());
for (DemoEntity* child = entity->GetChild(); child; child = child->GetSibling()) {
parentBones[stackIndex] = limbJoint;
childEntities[stackIndex] = child;
stackIndex++;
}
} else if (strstr(name, "effector")) {
// add effector
dMatrix gripperMatrix(entity->CalculateGlobalMatrix());
effector = new dKukaEffector(m_kinematicSolver, parentJoint, baseFrameBody, gripperMatrix);
effector->SetAsThreedof();
effector->SetLinearSpeed(2.0f);
effector->SetMaxLinearFriction(armRobotConfig[i].m_mass * DEMO_GRAVITY * 50.0f);
}
break;
}
}
}
// create the Animation tree for manipulation
DemoEntity* const effectoBone = model->Find("effector_arm");
dMatrix baseFrameMatrix(model->GetCurrentMatrix());
dMatrix effectorLocalMatrix(effectoBone->CalculateGlobalMatrix(model));
dVector upAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 1.0f, 0.0f, 1.0f)));
dVector planeAxis(baseFrameMatrix.UnrotateVector(dVector(0.0f, 0.0f, 1.0f, 1.0f)));
dEffectorTreeFixPose* const fixPose = new dEffectorTreeFixPose(baseFrameBody);
m_inputModifier = new dEffectorTreeInputModifier(fixPose, upAxis, planeAxis);
m_animTreeNode = new dEffectorTreeRoot(baseFrameBody, m_inputModifier);
// set base pose
dEffectorTreeInterface::dEffectorTransform frame;
frame.m_effector = effector;
frame.m_posit = effectorLocalMatrix.m_posit;
frame.m_rotation = dQuaternion(effectorLocalMatrix);
fixPose->GetPose().Append(frame);
m_animTreeNode->GetPose().Append(frame);
NewtonInverseDynamicsEndBuild(m_kinematicSolver);
}
