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