public void setLegBend(float stanceAngle, float swingAngle) { SimBiConState curState = lowLCon.getCurrentState(); Trajectory tmpTraj = curState.getTrajectory(1); if (tmpTraj.getTrajectoryComponentCount() > 0) { tmpTraj.getTrajectoryComponent(0).offset = swingAngle; } tmpTraj = curState.getTrajectory(2); if (tmpTraj.getTrajectoryComponentCount() > 0) { tmpTraj.getTrajectoryComponent(0).offset = stanceAngle; } tmpTraj = curState.getTrajectory(3); if (tmpTraj.getTrajectoryComponentCount() > 0) { tmpTraj.getTrajectoryComponent(0).offset = -swingAngle; } tmpTraj = curState.getTrajectory(4); if (tmpTraj.getTrajectoryComponentCount() > 0) { tmpTraj.getTrajectoryComponent(0).offset = -stanceAngle; } }
/** * This method is used to set the current FSM state of the controller to that of the index that * is passed in. */ protected void setFSMStateTo(int index) { if (index < 0) { FSMStateIndex = -1; } else if (index >= statesData.getStateCount()) { FSMStateIndex = statesData.getStateCount() - 1; } else { FSMStateIndex = index; } if (FSMStateIndex >= 0) { currentState = statesData.getState(FSMStateIndex); } }
virtual public void setUpperBodyPose(int index, float rootLean, float torsoLean, float headLean) { upperBodyPos[index] = rootLean; SimBiConState curState = lowLCon.getCurrentState(); Trajectory tmpTraj = curState.getTrajectory("root"); if (tmpTraj != null) { tmpTraj.getTrajectoryComponent(index).offset = rootLean; } tmpTraj = curState.getTrajectory("pelvis_waist"); if (tmpTraj != null) { tmpTraj.getTrajectoryComponent(index).offset = torsoLean; } tmpTraj = curState.getTrajectory("torso_head"); if (tmpTraj != null) { tmpTraj.getTrajectoryComponent(index).offset = headLean; } }
/** * This method is used to resolve the names (map them to their index) of the joints. */ private void resolveJoints(SimBiConState state) { string tmpName; for (int i = 0; i < state.getExternalForceCount(); i++) { ExternalForce ef = state.getExternalForce(i); //deal with the SWING_XXX' case if (ef.bName.StartsWith("SWING_")) { tmpName = ef.bName.Substring(6); tmpName = tmpName.Insert(0, "r"); ef.leftStanceARBIndex = getBodyIndex(tmpName); tmpName = tmpName.Remove(0, 1); tmpName = tmpName.Insert(0, "l"); ef.rightStanceARBIndex = getBodyIndex(tmpName); continue; } //deal with the STANCE_XXX' case if (ef.bName.StartsWith("STANCE_")) { tmpName = ef.bName.Substring(7); tmpName = tmpName.Insert(0, "l"); ef.leftStanceARBIndex = getBodyIndex(tmpName); tmpName = tmpName.Remove(0, 1); tmpName = tmpName.Insert(0, "r"); ef.rightStanceARBIndex = getBodyIndex(tmpName); continue; } //if we get here, it means it is just the name... ef.leftStanceARBIndex = getBodyIndex(ef.bName); ef.rightStanceARBIndex = ef.leftStanceARBIndex; } for (int i = 0; i < state.getTrajectoryCount(); i++) { Trajectory jt = state.getTrajectory(i); //deal with the 'root' special case if (jt.jName == "root") { jt.leftStanceIndex = jt.rightStanceIndex = -1; } else if (jt.jName.StartsWith("SWING_")) { tmpName = jt.jName.Substring(6); tmpName = tmpName.Insert(0, "r"); jt.leftStanceIndex = getJointIndex(tmpName); tmpName = tmpName.Remove(0, 1); tmpName = tmpName.Insert(0, "l"); jt.rightStanceIndex = getJointIndex(tmpName); } else if (jt.jName.StartsWith("STANCE_")) { tmpName = jt.jName.Substring(7); tmpName = tmpName.Insert(0, "l"); jt.leftStanceIndex = getJointIndex(tmpName); tmpName = tmpName.Remove(0, 1); tmpName = tmpName.Insert(0, "r"); jt.rightStanceIndex = getJointIndex(tmpName); } else { //if we get here, it means it is just the name... jt.leftStanceIndex = getJointIndex(jt.jName); jt.rightStanceIndex = jt.leftStanceIndex; } if (jt.hitFeedback != null) { jt.hitFeedback.hitARBIndex = getRBIndexBySymbolicName(jt.jName, state.getStance()); } } }
public void addState(SimBiConState state_disown) { states.Add(state_disown); resolveJoints(state_disown); }
/** * This method loads all the pertinent information regarding the simbicon controller from a file. */ public void loadFromFile(string data) { if (data == null) { return; } //throwError("NULL file name provided."); StringReader f = new StringReader(data); if (f == null) { return; } //throwError("Could not open file: %s", data); //UnityThreadHelper.CreateThread(()=> //{ //to be able to load multiple controllers from multiple files, //we will use this offset to make sure that the state numbers //mentioned in each input file are updated correctly int stateOffset = this.states.Count; SimBiConState tempState; int tempStateNr = -1; //have a temporary buffer used to read the file line by line... string buffer = f.ReadLine(); while (buffer != null) { if (buffer.Length > 195) { break; } string line = buffer.Trim(); int lineType = ConUtils.getConLineType(line); string[] nrParams = line.Split(new char[] { ' ' }, System.StringSplitOptions.RemoveEmptyEntries); switch (lineType) { case ConUtils.CON_STATE_START: tempState = new SimBiConState(); tempStateNr = int.Parse(nrParams[1]); states.Add(tempState); tempState.readState(f, stateOffset); //now we have to resolve all the joint names (i.e. figure out which joints they apply to). resolveJoints(tempState); //UnityThreadHelper.Dispatcher.Dispatch(() => onDataLoadProcess(tempStateNr + 1)); break; case ConUtils.CON_NOT_IMPORTANT: break; default: break; } buffer = f.ReadLine(); } f.Close(); onDataLoadFinished(states.Count); //UnityThreadHelper.Dispatcher.Dispatch(() => onDataLoadFinished(states.Count)); //}); }