@Override public void setFullRobotModel(FullRobotModel fullRobotModel) { revoluteJoints.clear(); OneDoFJoint[] revoluteJointsArray = fullRobotModel.getOneDoFJoints(); for (OneDoFJoint revoluteJoint : revoluteJointsArray) { String name = revoluteJoint.getName(); OneDegreeOfFreedomJoint oneDoFJoint = robot.getOneDegreeOfFreedomJoint(name); ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair = new ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint>(oneDoFJoint, revoluteJoint); this.revoluteJoints.add(jointPair); } rootJointPair = new ImmutablePair<FloatingJoint, FloatingInverseDynamicsJoint>( robot.getRootJoint(), fullRobotModel.getRootJoint()); }
@Override public void doControl() { super.doControl(); for (RobotSide robotSide : RobotSide.values()) { GroundContactPointsSlipper groundContactPointsSlipper = groundContactPointsSlippers.get(robotSide); switch (slipStateMap.get(robotSide).getEnumValue()) { case NO_CONTACT: { if (footTouchedDown(robotSide)) { if (doSlipThisStance()) { slipStateMap.get(robotSide).set(SlipState.CONTACT_WILL_SLIP); touchdownTimeForSlipMap.get(robotSide).set(robot.getTime()); } else // Wait till foot lift back up before allowing a slip. { slipStateMap.get(robotSide).set(SlipState.CONTACT); } } break; } case CONTACT_WILL_SLIP: { if (robot.getTime() > (touchdownTimeForSlipMap.get(robotSide).getDoubleValue() + nextSlipAfterTimeDelta.getDoubleValue())) { if (slipStateMap.get(robotSide.getOppositeSide()).getEnumValue() == SlipState.CONTACT_SLIP) { // Stop other foot from slipping, two slipping feet no implemented yet groundContactPointsSlipper.setDoSlip(false); slipStateMap.get(robotSide.getOppositeSide()).set(SlipState.CONTACT_DONE_SLIP); } slipStateMap.get(robotSide).set(SlipState.CONTACT_SLIP); startSlipping(robotSide); } break; } case CONTACT_SLIP: { if (groundContactPointsSlipper.isDoneSlipping()) { slipStateMap.get(robotSide).set(SlipState.CONTACT_DONE_SLIP); } break; } case CONTACT_DONE_SLIP: case CONTACT: { if (footLiftedUp(robotSide)) { slipStateMap.get(robotSide).set(SlipState.NO_CONTACT); } break; } } } }
public PerfectSimulatedOutputWriter(FloatingRootJointRobot robot, FullRobotModel fullRobotModel) { this.name = robot.getName() + "SimulatedSensorReader"; this.robot = robot; setFullRobotModel(fullRobotModel); }
public PerfectSimulatedOutputWriter(FloatingRootJointRobot robot) { this.name = robot.getName() + "SimulatedSensorReader"; this.robot = robot; }