public SimBiController(Character b) : base(b) { //if (b == null) //throwError("Cannot create a SIMBICON controller if there is no associated biped!!"); //characters controlled by a simbicon controller are assumed to have: 2 feet lLowerLeg = b.getARBByName("lLowerLeg"); rLowerLeg = b.getARBByName("rLowerLeg"); lUpperLeg = b.getARBByName("lUpperLeg"); rUpperLeg = b.getARBByName("rUpperLeg"); lFoot = lLowerLeg; rFoot = rLowerLeg; lHand = b.getARBByName("lLowerArm"); rHand = b.getARBByName("rLowerArm"); //and two hips connected to the root //Joint lHip = b.getJointByName("lHip"); //Joint rHip = b.getJointByName("rHip"); lHipIndex = b.getJointIndex("lHip"); rHipIndex = b.getJointIndex("rHip"); root = b.getRoot(); torso = b.getARBByName("torso"); head = b.getARBByName("head"); statesData = null; comVelocity = new Vector3(); comPosition = new Vector3(); d = new Vector3(); v = new Vector3(); doubleStanceCOMError = new Vector3(); comOffsetSagittal = 0; comOffsetCoronal = 0; qRootD = Quaternion.identity; isFreeHeading = true; isAddFootJoint = false; setStance(SimGlobals.LEFT_STANCE); phi = 0; setFSMStateTo(-1); prevFSMState = -1; nextFSMState = -1; isLeftReverseStance = true; bodyTouchedTheGround = false; lFootTouchedTheGround = false; rFootTouchedTheGround = false; twoFootTouchedGround = false; startingState = 1; startingStance = SimGlobals.LEFT_STANCE; }
/** * This method is used to set the stance */ public void setStance(int newStance) { stance = newStance; if (stance == SimGlobals.LEFT_STANCE) { stanceFoot = lFoot; swingFoot = rFoot; stanceHand = lHand; swingHand = rHand; swingHipIndex = rHipIndex; stanceHipIndex = lHipIndex; } else { stanceFoot = rFoot; swingFoot = lFoot; stanceHand = rHand; swingHand = lHand; swingHipIndex = lHipIndex; stanceHipIndex = rHipIndex; } }
public void evaluateJointTargets() { ReducedCharacterState poseRS = new ReducedCharacterState(desiredPose); //d and v are specified in the rotation (heading) invariant coordinate frame updateDAndV(); //there are two stages here. First we will compute the pose (i.e. relative orientations), using the joint trajectories for the current state //and then we will compute the PD torques that are needed to drive the links towards their orientations - here the special case of the //swing and stance hips will need to be considered //always start from a neutral desired pose, and build from there... root.setExternalForce(Vector3.zero); root.setExternalTorque(Vector3.zero); CWJoint j; for (int i = 0; i < jointCount; i++) { j = character.getJoint(i); j.getChild().setExternalForce(Vector3.zero); j.getChild().setExternalTorque(Vector3.zero); poseRS.setJointRelativeOrientation(Quaternion.identity, i); poseRS.setJointRelativeAngVelocity(Vector3.zero, i); } float phiToUse = phi; //make sure that we only evaluate trajectories for phi values between 0 and 1 if (phiToUse > 1) { phiToUse = 1; } Vector3 force = Vector3.zero; Vector3 torque = Vector3.zero; // First compute external forces for (int i = 0; i < currentState.getExternalForceCount(); i++) { ExternalForce ef = currentState.getExternalForce(i); CWRigidBody arb = character.getArticulatedRigidBody(ef.getARBIndex(stance)); force = Vector3.zero; force += character.getLocalFrontAxis() * ef.forceX.evaluate_catmull_rom(phiToUse); force += character.getLocalUpAxis() * ef.forceY.evaluate_catmull_rom(phiToUse); force += character.getLocalLeftAxis() * ef.forceZ.evaluate_catmull_rom(phiToUse); force = characterFrame * force; arb.addExternalForce(force); torque = Vector3.zero; torque += character.getLocalFrontAxis() * ef.torqueX.evaluate_catmull_rom(phiToUse); torque += character.getLocalUpAxis() * ef.torqueY.evaluate_catmull_rom(phiToUse); torque += character.getLocalLeftAxis() * ef.torqueZ.evaluate_catmull_rom(phiToUse); torque = characterFrame * torque; arb.addExternalTorque(torque); } //Debug.Log(Random.value + "ffffffffffff "+curState.getTrajectoryCount()); for (int i = 0; i < currentState.getTrajectoryCount(); i++) { Trajectory tra = currentState.getTrajectory(i); //now we have the desired rotation angle and axis, so we need to see which joint this is intended for int jIndex = tra.getJointIndex(stance, isLeftReverseStance); //Debug.Log(Random.value + "00000000000 "+(curState.sTraj[i] as Trajectory).getJointName()); j = character.getJoint(jIndex); //get the desired joint orientation to track - include the feedback if necessary/applicable Vector3 d0 = new Vector3(); Vector3 v0 = new Vector3(); computeD0(phiToUse, ref d0); computeV0(phiToUse, ref v0); //if the index is -1, it must mean it's the root's trajectory. Otherwise we should give an error if (jIndex == -1) { qRootD = tra.evaluateTrajectory(this, j, stance, phiToUse, d - d0, v - v0); } else { Quaternion newOrientation = tra.evaluateTrajectory(this, j, stance, phiToUse, d - d0, v - v0); j.setRelToRootFrame(tra.relToCharFrame); poseRS.setJointRelativeOrientation(newOrientation, jIndex); } if (tra.hitFeedback != null) { CWRigidBody arb = character.getArticulatedRigidBody(tra.hitFeedback.hitARBIndex); if (phiToUse > tra.hitFeedback.minTime && arb.contactPoints.Count > 0) { ContactPointInfo cp = (ContactPointInfo)arb.contactPoints[0]; onHitOtherSBCharacter(tra.hitFeedback, cp); } } } }