Beispiel #1
0
	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);
	}