@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;
}
