void PostUpdate(dFloat timestep, int threadIndex) { int count = 1; void* nodes[32]; dMatrix parentMatrix[32]; nodes[0] = NewtonInverseDynamicsGetRoot(m_kinematicSolver); parentMatrix[0] = dGetIdentityMatrix(); NewtonWorld* const world = GetManager()->GetWorld(); DemoEntityManager* const scene = (DemoEntityManager*)NewtonWorldGetUserData(world); while (count) { dMatrix matrix; count --; void* const rootNode = nodes[count]; NewtonBody* const body = NewtonInverseDynamicsGetBody(m_kinematicSolver, rootNode); NewtonBodyGetMatrix (body, &matrix[0][0]); dMatrix localMatrix (matrix * parentMatrix[count]); DemoEntity* const ent = (DemoEntity*)NewtonBodyGetUserData(body); dQuaternion rot(localMatrix); ent->SetMatrix(*scene, rot, localMatrix.m_posit); matrix = matrix.Inverse(); for (void* node = NewtonInverseDynamicsGetFirstChildNode(m_kinematicSolver, rootNode); node; node = NewtonInverseDynamicsGetNextChildNode(m_kinematicSolver, node)) { nodes[count] = node; parentMatrix[count] = matrix; count ++; } } }
dCustomInverseDynamicsEffector* AddLeg(DemoEntityManager* const scene, void* const rootNode, const dMatrix& matrix, dFloat partMass, dFloat limbLenght) { NewtonBody* const parent = NewtonInverseDynamicsGetBody(m_kinematicSolver, rootNode); dFloat inertiaScale = 4.0f; // make limb base dMatrix baseMatrix(dRollMatrix(dPi * 0.5f)); dMatrix cylinderMatrix(baseMatrix * matrix); NewtonBody* const base = CreateCylinder(scene, cylinderMatrix, partMass, inertiaScale, 0.2f, 0.1f); dCustomInverseDynamics* const baseHinge = new dCustomInverseDynamics(cylinderMatrix, base, parent); baseHinge->SetJointTorque(1000.0f); baseHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi); void* const baseHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, rootNode, baseHinge->GetJoint()); //make limb forward arm dMatrix forwardArmMatrix(dPitchMatrix(-30.0f * dDegreeToRad)); dVector forwardArmSize(limbLenght * 0.25f, limbLenght * 0.25f, limbLenght, 0.0f); forwardArmMatrix.m_posit += forwardArmMatrix.m_right.Scale(forwardArmSize.m_z * 0.5f); NewtonBody* const forwardArm = CreateBox(scene, forwardArmMatrix * matrix, forwardArmSize, partMass, inertiaScale); dMatrix forwardArmPivot(forwardArmMatrix); forwardArmPivot.m_posit -= forwardArmMatrix.m_right.Scale(forwardArmSize.m_z * 0.5f); dCustomInverseDynamics* const forwardArmHinge = new dCustomInverseDynamics(forwardArmPivot * matrix, forwardArm, base); forwardArmHinge->SetJointTorque(1000.0f); forwardArmHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi); void* const forwardArmHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, baseHingeNode, forwardArmHinge->GetJoint()); //make limb forward arm dMatrix armMatrix(dPitchMatrix(-90.0f * dDegreeToRad)); dFloat armSize = limbLenght * 1.25f; armMatrix.m_posit += forwardArmMatrix.m_right.Scale(limbLenght); armMatrix.m_posit.m_y -= armSize * 0.5f; NewtonBody* const arm = CreateCapsule(scene, armMatrix * matrix, partMass, inertiaScale, armSize * 0.2f, armSize); dMatrix armPivot(armMatrix); armPivot.m_posit.m_y += armSize * 0.5f; dCustomInverseDynamics* const armHinge = new dCustomInverseDynamics(armPivot * matrix, arm, forwardArm); armHinge->SetJointTorque(1000.0f); armHinge->SetTwistAngle(-0.5f * dPi, 0.5f * dPi); void* const armHingeNode = NewtonInverseDynamicsAddChildNode(m_kinematicSolver, forwardArmHingeNode, armHinge->GetJoint()); dMatrix effectorMatrix(dGetIdentityMatrix()); effectorMatrix.m_posit = armPivot.m_posit; effectorMatrix.m_posit.m_y -= armSize; dHexapodEffector* const effector = new dHexapodEffector(m_kinematicSolver, armHingeNode, parent, effectorMatrix * matrix); effector->SetAsThreedof(); effector->SetMaxLinearFriction(partMass * DEMO_GRAVITY * 10.0f); effector->SetMaxAngularFriction(partMass * DEMO_GRAVITY * 10.0f); return effector; }
dCustomJoint::dCustomJoint(NewtonInverseDynamics* const invDynSolver, void* const invDynNode) :dCustomAlloc() ,m_localMatrix0(dGetIdentityMatrix()) ,m_localMatrix1(dGetIdentityMatrix()) { m_joint = NULL; m_body1 = NULL; m_maxDof = 6; m_autoDestroy = 0; m_stiffness = 1.0f; m_body0 = NewtonInverseDynamicsGetBody (invDynSolver, invDynNode); m_world = NewtonBodyGetWorld(m_body0); m_joint = NewtonInverseDynamicsCreateEffector(invDynSolver, invDynNode, SubmitConstraints); NewtonJointSetUserData(m_joint, this); NewtonJointSetDestructor(m_joint, Destructor); m_userData = NULL; m_userDestructor = NULL; }
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); }