@Override
public ArrayList<FootstepData> getStepsAlongPath(RobotSide firstStepSide) {
if (overheadPathList.isEmpty()) {
throw new RuntimeException(this.getClass().getSimpleName() + ": No Path for generator");
}
// if (!hasHeightMap())
// {
// TODO: @Agraber do simple planning assuming flat ground at the initial footstep height
// throw new RuntimeException(this.getClass().getSimpleName() + ": No HeightMap");
// }
ArrayList<FootstepData> generatedSteps = new ArrayList<>();
RobotSide lastStepSide = firstStepSide.getOppositeSide();
SideDependentList<FootstepData> lastFootsteps = initialFeet;
for (FootstepOverheadPath overheadPath : overheadPathList) {
// for each segment, generate a path to the end, then use that as the initial for the next
super.initialize(lastFootsteps, overheadPath);
List<FootstepData> segmentFootsteps = super.getStepsAlongPath(lastStepSide.getOppositeSide());
for (FootstepData footstep : segmentFootsteps) {
generatedSteps.add(footstep);
lastStepSide = footstep.getRobotSide();
lastFootsteps.put(lastStepSide, footstep);
}
// check that the segment reached the goal
FramePose2d segmentGoal = overheadPath.getPoseAtDistance(overheadPath.getTotalDistance());
if (!isGoalFootstep(lastFootsteps.get(lastStepSide), segmentGoal, horizontalDistance)) {
break;
}
}
return generatedSteps;
}
public void initializeDesiredFootstep(RobotSide supportLegSide) {
RobotSide swingLegSide = supportLegSide.getOppositeSide();
ReferenceFrame supportAnkleZUpFrame = ankleZUpFrames.get(supportLegSide);
ReferenceFrame supportAnkleFrame = ankleFrames.get(supportLegSide);
computeDistanceAndAngleToDestination(
supportAnkleZUpFrame, swingLegSide, desiredDestination.getFramePoint2dCopy());
if (distanceToDestination.getDoubleValue() < DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE) {
numberBlindStepsInPlace.increment();
}
Matrix3d footToWorldRotation =
computeDesiredFootRotation(
angleToDestination.getDoubleValue(), swingLegSide, supportAnkleFrame);
FramePoint footstepPosition =
getDesiredFootstepPositionCopy(
supportAnkleZUpFrame,
supportAnkleFrame,
swingLegSide,
desiredDestination.getFramePoint2dCopy(),
footToWorldRotation);
setYoVariables(swingLegSide, footToWorldRotation, footstepPosition);
}
private SideDependentList<ForceSensorDataReadOnly> createWristForceSensors(
ForceSensorDataHolderReadOnly forceSensorDataHolder) {
if (wristSensorNames == null) return null;
SideDependentList<ForceSensorDataReadOnly> wristForceSensors = new SideDependentList<>();
for (RobotSide robotSide : RobotSide.values) {
if (wristSensorNames.get(robotSide) == null) {
return null;
}
ForceSensorDataReadOnly wristForceSensor =
forceSensorDataHolder.getByName(wristSensorNames.get(robotSide));
wristForceSensors.put(robotSide, wristForceSensor);
}
return wristForceSensors;
}
protected void setupTest(DRCRobotModel robotModel)
throws SimulationExceededMaximumTimeException, InterruptedException {
boolean runMultiThreaded = false;
setupTrack(runMultiThreaded, robotModel);
FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
pushRobotController =
new PushRobotController(
drcFlatGroundWalkingTrack.getDrcSimulation().getRobot(), fullRobotModel);
if (VISUALIZE_FORCE) {
drcFlatGroundWalkingTrack
.getSimulationConstructionSet()
.addYoGraphic(pushRobotController.getForceVisualizer());
}
SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
BooleanYoVariable enable =
(BooleanYoVariable) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery");
// enable push recovery
enable.set(true);
for (RobotSide robotSide : RobotSide.values) {
String prefix = fullRobotModel.getFoot(robotSide).getName();
scs.getVariable(prefix + "FootControlModule", prefix + "State");
@SuppressWarnings("unchecked")
final EnumYoVariable<WalkingStateEnum> walkingState =
(EnumYoVariable<WalkingStateEnum>)
scs.getVariable("WalkingHighLevelHumanoidController", "walkingState");
doubleSupportStartConditions.put(
robotSide, new DoubleSupportStartCondition(walkingState, robotSide));
}
}
@Override
public void receivedPacket(SteeringWheelInformationPacket packet) {
if (packet == null) return;
if (!PacketValidityChecker.validateSteeringWheelInformationPacket(
packet, steeringWheelIdAtomic, globalDataProducer)) return;
RobotSide robotSide = packet.getRobotSide();
steeringWheelCenterAtomic.get(robotSide).set(packet.getSteeringWheelCenter());
steeringWheelRotationAxisAtomic.get(robotSide).set(packet.getSteeringWheelRotationAxis());
steeringWheelZeroAxisAtomic.get(robotSide).set(packet.getSteeringWheelZeroAxis());
steeringWheelRadiusAtomic.get(robotSide).set(packet.getSteeringWheelRadius());
graspOffsetFromCotnrolFrameAtomic.get(robotSide).set(packet.getGraspOffsetFromControlFrame());
desiredSteeringSpeedAtomic.get(robotSide).set(packet.getDesiredSteeringSpeed());
steeringWheelIdAtomic.get(robotSide).set(packet.getSteeringWheelId());
hasReceivedNewSteeringWheelInformation.get(robotSide).set(true);
}
public KinematicsBasedFootSwitch(
String footName,
SideDependentList<? extends ContactablePlaneBody> bipedFeet,
double switchZThreshold,
double totalRobotWeight,
RobotSide side,
YoVariableRegistry parentRegistry) {
registry = new YoVariableRegistry(footName + getClass().getSimpleName());
foot = bipedFeet.get(side);
ContactablePlaneBody oppositeFoot = bipedFeet.get(side.getOppositeSide());
otherFeet = new ContactablePlaneBody[] {oppositeFoot};
this.totalRobotWeight = totalRobotWeight;
hitGround = new BooleanYoVariable(footName + "hitGround", registry);
fixedOnGround = new BooleanYoVariable(footName + "fixedOnGround", registry);
soleZ = new DoubleYoVariable(footName + "soleZ", registry);
ankleZ = new DoubleYoVariable(footName + "ankleZ", registry);
this.switchZThreshold = new DoubleYoVariable(footName + "footSwitchZThreshold", registry);
this.switchZThreshold.set(switchZThreshold);
yoResolvedCoP = new YoFramePoint2d(footName + "ResolvedCoP", "", foot.getSoleFrame(), registry);
parentRegistry.addChild(registry);
}
private void startSlipping(RobotSide robotSide) {
GroundContactPointsSlipper groundContactPointsSlipper =
groundContactPointsSlippers.get(robotSide);
generateRandomSlipParamters();
groundContactPointsSlipper.setGroundContactPoints(groundContactPointsMap.get(robotSide));
groundContactPointsSlipper.setPercentToSlipPerTick(nextSlipPercentSlipPerTick.getDoubleValue());
groundContactPointsSlipper.setDoSlip(true);
groundContactPointsSlipper.setSlipTranslation(nextTranslationToSlip.getVector3dCopy());
groundContactPointsSlipper.setSlipRotationYawPitchRoll(nextRotationToSlip.getYawPitchRoll());
// System.out.println("Slip of " + robotSide.getLowerCaseName() + " foot with amount" +
// nextTranslationToSlip.getVector3dCopy().toString()
// + " with rotation amount " +
// nextRotationToSlip.getFrameOrientationCopy().toStringAsYawPitchRoll()
// + " with slippercentage per tick " +
// nextSlipPercentSlipPerTick.getDoubleValue() + ", " + nextSlipAfterTimeDelta.getDoubleValue()
// + " [s] after touchdown.");
}
public double getSteeringWheelRadius(RobotSide robotSide) {
return steeringWheelRadiusAtomic.get(robotSide).get();
}
public boolean checkForNewDesiredAbsoluteSteeringAngle(RobotSide robotSide) {
return hasReceivedNewDesiredSteeringAngle.get(robotSide).getAndSet(false);
}
public boolean checkForNewSteeringWheelInformation(RobotSide robotSide) {
return hasReceivedNewSteeringWheelInformation.get(robotSide).getAndSet(false);
}
public SlipRandomOnNextStepPerturber(HumanoidFloatingRootJointRobot robot) {
super("SlipRandomOnNextStepPerturber");
String name = "SlipRandom";
this.robot = robot;
groundContactPointsSlippers = new SideDependentList<GroundContactPointsSlipper>();
for (RobotSide robotSide : RobotSide.values()) {
DoubleYoVariable touchdownTimeForSlip =
new DoubleYoVariable(
robotSide.getCamelCaseNameForStartOfExpression()
+ "TouchdownTimeForSlip"
+ robotSide.getCamelCaseNameForMiddleOfExpression(),
registry);
touchdownTimeForSlipMap.put(robotSide, touchdownTimeForSlip);
EnumYoVariable<SlipState> slipState =
new EnumYoVariable<SlipState>(
name + "SlipState" + robotSide.getCamelCaseNameForMiddleOfExpression(),
registry,
SlipState.class);
slipState.set(SlipState.NO_CONTACT);
slipStateMap.put(robotSide, slipState);
groundContactPointsMap.put(robotSide, robot.getFootGroundContactPoints(robotSide));
GroundContactPointsSlipper groundContactPointsSlipper =
new GroundContactPointsSlipper(robotSide.getLowerCaseName());
groundContactPointsSlippers.put(robotSide, groundContactPointsSlipper);
this.addRobotController(groundContactPointsSlipper);
}
this.minSlipAfterTimeDelta = new DoubleYoVariable(name + "MinSlipAfterTimeDelta", registry);
this.maxSlipAfterTimeDelta = new DoubleYoVariable(name + "MaxSlipAfterTimeDelta", registry);
this.nextSlipAfterTimeDelta = new DoubleYoVariable(name + "NextSlipAfterTimeDelta", registry);
this.minSlipPercentSlipPerTick =
new DoubleYoVariable(name + "MinSlipPercentSlipPerTick", registry);
this.maxSlipPercentSlipPerTick =
new DoubleYoVariable(name + "MaxSlipPercentSlipPerTick", registry);
this.nextSlipPercentSlipPerTick =
new DoubleYoVariable(name + "NextSlipPercentSlipPerTick", registry);
this.slipNextStep = new BooleanYoVariable(name + "SlipNextStep", registry);
maxTranslationToSlipNextStep =
new YoFrameVector(
name + "MaxTranslationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry);
minTranslationToSlipNextStep =
new YoFrameVector(
name + "MinTranslationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry);
nextTranslationToSlip =
new YoFrameVector(name + "NextTranslationToSlip", ReferenceFrame.getWorldFrame(), registry);
maxRotationToSlipNextStep =
new YoFrameOrientation(
name + "MaxRotationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry);
minRotationToSlipNextStep =
new YoFrameOrientation(
name + "MinRotationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry);
nextRotationToSlip =
new YoFrameOrientation(
name + "NextRotationToSlip", ReferenceFrame.getWorldFrame(), registry);
setTranslationRangeToSlipNextStep(new double[] {0.0, 0.0, 0.0}, new double[] {0.05, 0.05, 0.0});
setRotationRangeToSlipNextStep(new double[] {0.0, 0.0, 0.0}, new double[] {0.3, 0.15, 0.1});
setSlipAfterStepTimeDeltaRange(0.01, 0.10);
setSlipPercentSlipPerTickRange(0.01, 0.05);
setProbabilityOfSlip(1.0);
}
public boolean heelToeOffAhead(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getX()
> GCpointsHeel.get(robotSide.getOppositeSide()).getX();
}
public double getDesiredSteeringSpeed(RobotSide robotSide) {
return desiredSteeringSpeedAtomic.get(robotSide).get();
}
public abstract class DRCPushRecoveryMultiStepTest implements MultiRobotTestInterface {
private static final SimulationTestingParameters simulationTestingParameters =
SimulationTestingParameters.createFromEnvironmentVariables();
private DRCFlatGroundWalkingTrack drcFlatGroundWalkingTrack;
private static final boolean VISUALIZE_FORCE = false;
private static final double PUSH_DELAY = 0.5;
protected PushRobotController pushRobotController;
protected BlockingSimulationRunner blockingSimulationRunner;
protected double forceMagnitude;
protected double forceDuration;
protected SideDependentList<StateTransitionCondition> doubleSupportStartConditions =
new SideDependentList<>();
StateTransitionCondition pushCondition = doubleSupportStartConditions.get(RobotSide.LEFT);
@Before
public void showMemoryUsageBeforeTest() {
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " before test.");
}
@After
public void destroySimulationAndRecycleMemory() {
if (simulationTestingParameters.getKeepSCSUp()) {
ThreadTools.sleepForever();
}
// Do this here in case a test fails. That way the memory will be recycled.
if (drcFlatGroundWalkingTrack != null) {
drcFlatGroundWalkingTrack.destroySimulation();
drcFlatGroundWalkingTrack = null;
}
if (blockingSimulationRunner != null) {
blockingSimulationRunner.destroySimulation();
blockingSimulationRunner = null;
}
pushRobotController = null;
doubleSupportStartConditions = null;
pushCondition = null;
MemoryTools.printCurrentMemoryUsageAndReturnUsedMemoryInMB(
getClass().getSimpleName() + " after test.");
}
@Ignore("Needs to be improved")
@ContinuousIntegrationTest(estimatedDuration = 67.1)
@Test(timeout = 340000)
public void testMultiStepForwardAndContinueWalking()
throws SimulationExceededMaximumTimeException, InterruptedException,
ControllerFailureException {
BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
setupTest(getRobotModel());
SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
setForwardPushParameters();
Vector3d forceDirection = new Vector3d(1.0, 0.0, 0.0);
blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0);
BooleanYoVariable walk =
(BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");
// disable walking
walk.set(false);
blockingSimulationRunner.simulateAndBlock(4.0);
// push the robot
pushRobotController.applyForceDelayed(
pushCondition, PUSH_DELAY, forceDirection, forceMagnitude, forceDuration);
// simulate for a little while longer
blockingSimulationRunner.simulateAndBlock(forceDuration + 6.0);
// re-enable walking
walk.set(true);
blockingSimulationRunner.simulateAndBlock(6.0);
Point3d center = new Point3d(0.0, 0.0, 0.8882009563211146);
Vector3d plusMinusVector =
new Vector3d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5);
BoundingBox3d boundingBox =
BoundingBox3d.createUsingCenterAndPlusMinusVector(center, plusMinusVector);
DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(
boundingBox, drcFlatGroundWalkingTrack.getDrcSimulation().getRobot());
createVideo(scs);
BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
}
@ContinuousIntegrationTest(estimatedDuration = 53.2)
@Test(timeout = 270000)
public void testMultiStepBackwardAndContinueWalking()
throws SimulationExceededMaximumTimeException, InterruptedException,
ControllerFailureException {
BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows());
setupTest(getRobotModel());
SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
setBackwardPushParameters();
Vector3d forceDirection = new Vector3d(1.0, 0.0, 0.0);
blockingSimulationRunner = new BlockingSimulationRunner(scs, 1000.0);
BooleanYoVariable walk =
(BooleanYoVariable) scs.getVariable("ComponentBasedFootstepDataMessageGenerator", "walk");
// disable walking
walk.set(false);
blockingSimulationRunner.simulateAndBlock(1.0);
// push the robot
pushRobotController.applyForceDelayed(
pushCondition, PUSH_DELAY, forceDirection, forceMagnitude, forceDuration);
// simulate for a little while longer
blockingSimulationRunner.simulateAndBlock(forceDuration + 5.0);
// re-enable walking
walk.set(true);
blockingSimulationRunner.simulateAndBlock(6.0);
Point3d center = new Point3d(0.0, 0.0, 0.8882009563211146);
Vector3d plusMinusVector =
new Vector3d(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, 0.5);
BoundingBox3d boundingBox =
BoundingBox3d.createUsingCenterAndPlusMinusVector(center, plusMinusVector);
DRCSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(
boundingBox, drcFlatGroundWalkingTrack.getDrcSimulation().getRobot());
createVideo(scs);
BambooTools.reportTestFinishedMessage(simulationTestingParameters.getShowWindows());
}
protected void setupTest(DRCRobotModel robotModel)
throws SimulationExceededMaximumTimeException, InterruptedException {
boolean runMultiThreaded = false;
setupTrack(runMultiThreaded, robotModel);
FullHumanoidRobotModel fullRobotModel = robotModel.createFullRobotModel();
pushRobotController =
new PushRobotController(
drcFlatGroundWalkingTrack.getDrcSimulation().getRobot(), fullRobotModel);
if (VISUALIZE_FORCE) {
drcFlatGroundWalkingTrack
.getSimulationConstructionSet()
.addYoGraphic(pushRobotController.getForceVisualizer());
}
SimulationConstructionSet scs = drcFlatGroundWalkingTrack.getSimulationConstructionSet();
BooleanYoVariable enable =
(BooleanYoVariable) scs.getVariable("PushRecoveryControlModule", "enablePushRecovery");
// enable push recovery
enable.set(true);
for (RobotSide robotSide : RobotSide.values) {
String prefix = fullRobotModel.getFoot(robotSide).getName();
scs.getVariable(prefix + "FootControlModule", prefix + "State");
@SuppressWarnings("unchecked")
final EnumYoVariable<WalkingStateEnum> walkingState =
(EnumYoVariable<WalkingStateEnum>)
scs.getVariable("WalkingHighLevelHumanoidController", "walkingState");
doubleSupportStartConditions.put(
robotSide, new DoubleSupportStartCondition(walkingState, robotSide));
}
}
private void setupTrack(boolean runMultiThreaded, DRCRobotModel robotModel) {
DRCGuiInitialSetup guiInitialSetup =
new DRCGuiInitialSetup(true, false, simulationTestingParameters);
GroundProfile3D groundProfile = new FlatGroundProfile();
DRCSCSInitialSetup scsInitialSetup =
new DRCSCSInitialSetup(groundProfile, robotModel.getSimulateDT());
// scsInitialSetup.setInitializeEstimatorToActual(true);
scsInitialSetup.setDrawGroundProfile(true);
scsInitialSetup.setRunMultiThreaded(runMultiThreaded);
DRCRobotInitialSetup<HumanoidFloatingRootJointRobot> robotInitialSetup =
robotModel.getDefaultRobotInitialSetup(0.0, 0.0);
drcFlatGroundWalkingTrack =
new DRCFlatGroundWalkingTrack(
robotInitialSetup, guiInitialSetup, scsInitialSetup, true, false, robotModel);
drcFlatGroundWalkingTrack.getDrcSimulation();
}
private class DoubleSupportStartCondition implements StateTransitionCondition {
private final EnumYoVariable<WalkingStateEnum> walkingState;
private final RobotSide side;
public DoubleSupportStartCondition(
EnumYoVariable<WalkingStateEnum> walkingState, RobotSide side) {
this.walkingState = walkingState;
this.side = side;
}
@Override
public boolean checkCondition() {
if (side == RobotSide.LEFT) {
return (walkingState.getEnumValue() == WalkingStateEnum.TO_STANDING)
|| (walkingState.getEnumValue() == WalkingStateEnum.TO_WALKING_LEFT_SUPPORT);
} else {
return (walkingState.getEnumValue() == WalkingStateEnum.TO_STANDING)
|| (walkingState.getEnumValue() == WalkingStateEnum.TO_WALKING_RIGHT_SUPPORT);
}
}
}
private void createVideo(SimulationConstructionSet scs) {
if (simulationTestingParameters.getCreateSCSVideos()) {
BambooTools.createVideoAndDataWithDateTimeClassMethodAndShareOnSharedDriveIfAvailable(
getSimpleRobotName(), scs, 1);
}
}
protected abstract void setForwardPushParameters();
protected abstract void setBackwardPushParameters();
}
public class Step6IDandSCSRobot_pinKnee extends Robot {
/** Variables */
// ID
private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
private final ReferenceFrame elevatorFrame =
ReferenceFrame.constructFrameWithUnchangingTransformToParent(
"elevator", worldFrame, new RigidBodyTransform());
private FramePoint bodyPosition = new FramePoint();
private final Vector3d jointAxesPinJoints =
new Vector3d(0.0, 1.0, 0.0); // rotate around Y-axis (for revolute joints)
private final RigidBody elevator;
private final SideDependentList<EnumMap<JointNames, OneDoFJoint>> allLegJoints =
SideDependentList.createListOfEnumMaps(JointNames.class); // Includes the ankle!
private final LinkedHashMap<OneDoFJoint, OneDegreeOfFreedomJoint> idToSCSLegJointMap =
new LinkedHashMap<
OneDoFJoint,
OneDegreeOfFreedomJoint>(); // makes the SCS joints available without another side
// dependent enum
private final SideDependentList<EnumMap<LinkNames, RigidBody>> allLegRigidBodies =
SideDependentList.createListOfEnumMaps(LinkNames.class);
private final SixDoFJoint bodyJointID;
// SCS
private final FloatingPlanarJoint bodyJointSCS;
SideDependentList<GroundContactPoint> GCpointsHeel = new SideDependentList<GroundContactPoint>();
SideDependentList<GroundContactPoint> GCpointsToe = new SideDependentList<GroundContactPoint>();
private double cubeX = RobotParameters.BODY_DIMENSIONS.get(Axis.X);
private double cubeY = RobotParameters.BODY_DIMENSIONS.get(Axis.Y);
private double cubeZ = RobotParameters.BODY_DIMENSIONS.get(Axis.Z);
private double footX = RobotParameters.FOOT_DIMENSIONS.get(Axis.X);
private double footY = RobotParameters.FOOT_DIMENSIONS.get(Axis.Y);
private double footZ = RobotParameters.FOOT_DIMENSIONS.get(Axis.Z);
// GENERAL
private double bodyMass = RobotParameters.MASSES.get(LinkNames.BODY_LINK);
private double footMass = RobotParameters.MASSES.get(LinkNames.FOOT_LINK);
private double hipOffsetY = RobotParameters.BODY_DIMENSIONS.get(Axis.X) / 2.0;
private double kneeOffsetZ = -RobotParameters.LENGTHS.get(LinkNames.UPPER_LINK) + 0.1;
private double ankleOffsetZ = -RobotParameters.LENGTHS.get(LinkNames.LOWER_LINK);
private double footOffsetX = 0.15;
private double gcOffset = -footZ;
private double gcRadius = 0.03;
Vector3d comOffsetBody = new Vector3d(0.0, 0.0, cubeZ / 2.0);
Vector3d comOffsetFoot = new Vector3d(footX / 2.0 - 0.075, 0.0, -footZ / 2.0);
private double bodyZ, bodyX; // global so that it is created only once (avoid generating garbage)
public DoubleYoVariable qd_x;
private double initialBodyHeight =
RobotParameters.LENGTHS.get(LinkNames.UPPER_LINK)
+ RobotParameters.LENGTHS.get(LinkNames.LOWER_LINK)
- 0.1
+ footZ
- 0.029
+ 0.024;
/** Joints */
public Step6IDandSCSRobot_pinKnee() {
super("Robot");
/** **************** ID ROBOT ********************** */
elevator = new RigidBody("elevator", elevatorFrame);
bodyJointID = new SixDoFJoint(JointNames.BODY.getName(), elevator, elevatorFrame);
createAndAttachBodyRB(LinkNames.BODY_LINK, bodyJointID);
for (RobotSide robotSide : RobotSide.values) {
// HIP ID (location, joint, and rigidBody)
Vector3d hipOffset = new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0);
RevoluteJoint hipJointID =
ScrewTools.addRevoluteJoint(
JointNames.HIP.getName(),
bodyJointID.getSuccessor(),
hipOffset,
jointAxesPinJoints); // The parent rigid body of the hip joint is:
// bodyJointID.getSuccessor()
allLegJoints.get(robotSide).put(JointNames.HIP, hipJointID);
createAndAttachCylinderRB(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);
// KNEE ID
Vector3d kneeOffset = new Vector3d(0.0, 0.0, kneeOffsetZ);
RevoluteJoint kneeJointID =
ScrewTools.addRevoluteJoint(
JointNames.KNEE.getName(), hipJointID.getSuccessor(), kneeOffset, jointAxesPinJoints);
allLegJoints.get(robotSide).put(JointNames.KNEE, kneeJointID);
createAndAttachCylinderRB(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);
// ANKLE ID (location, joint, and rigidBody)
Vector3d ankleOffset = new Vector3d(0.0, 0.0, ankleOffsetZ);
RevoluteJoint ankleJointID =
ScrewTools.addRevoluteJoint(
JointNames.ANKLE.getName(),
kneeJointID.getSuccessor(),
ankleOffset,
jointAxesPinJoints);
allLegJoints.get(robotSide).put(JointNames.ANKLE, ankleJointID);
createAndAttachFootRB(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
}
/** **************** SCS ROBOT ********************** */
// BODY SCS
bodyJointSCS = new FloatingPlanarJoint(JointNames.BODY.getName(), this);
this.addRootJoint(bodyJointSCS);
createAndAttachBodyLink(LinkNames.BODY_LINK);
bodyJointSCS.setCartesianPosition(0.0, initialBodyHeight);
for (RobotSide robotSide : RobotSide.values) {
// HIP SCS
PinJoint hipJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.HIP.getName(),
new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0),
this,
jointAxesPinJoints);
hipJointSCS.setLimitStops(-0.75, 0.75, 1e3, 1e1);
// It is NOT necessary to set limits in the ID description because if the SCS description
// doesn't let the robot move passed a point the ID robot won't be able to pass it either
if (robotSide == RobotSide.LEFT) {
hipJointSCS.setQ(-0.6);
} else {
hipJointSCS.setQ(0.1);
}
bodyJointSCS.addJoint(hipJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.HIP), hipJointSCS);
createAndAttachCylinderLink(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);
// KNEE SCS
PinJoint kneeJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.KNEE.getName(),
new Vector3d(0.0, 0.0, kneeOffsetZ),
this,
jointAxesPinJoints);
kneeJointSCS.setLimitStops(-0.01, 1.8, 1e5, 1e3);
hipJointSCS.addJoint(kneeJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.KNEE), kneeJointSCS);
createAndAttachCylinderLink(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);
// ANKLE SCS
PinJoint ankleJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.ANKLE.getName(),
new Vector3d(0.0, 0.0, ankleOffsetZ),
this,
jointAxesPinJoints);
ankleJointSCS.setLimitStops(-0.7, 0.7, 1e3, 1e2);
if (robotSide == RobotSide.RIGHT) {
ankleJointSCS.setQ(-0.1);
}
kneeJointSCS.addJoint(ankleJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.ANKLE), ankleJointSCS);
// FEET SCS
createAndAttachFootLink(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
GroundContactPoint gcHeel =
new GroundContactPoint(
robotSide.getSideNameFirstLetter() + "gcHeel",
new Vector3d(-0.1, 0.0, gcOffset),
this);
GCpointsHeel.set(robotSide, gcHeel);
ankleJointSCS.addGroundContactPoint(gcHeel);
Graphics3DObject graphicsGCheel = ankleJointSCS.getLink().getLinkGraphics();
graphicsGCheel.identity();
graphicsGCheel.translate(-0.1, 0.0, gcOffset);
graphicsGCheel.addSphere(gcRadius, YoAppearance.Orange());
if (robotSide == RobotSide.RIGHT) {
setFStoTrue(robotSide);
}
GroundContactPoint gcToe =
new GroundContactPoint(
robotSide.getSideNameFirstLetter() + "gcToe", new Vector3d(0.4, 0.0, gcOffset), this);
GCpointsToe.set(robotSide, gcToe);
ankleJointSCS.addGroundContactPoint(gcToe);
Graphics3DObject graphics = ankleJointSCS.getLink().getLinkGraphics();
graphics.identity();
graphics.translate(0.4, 0.0, gcOffset);
graphics.addSphere(gcRadius, YoAppearance.DarkOliveGreen());
}
/** ************** SCS Ground Model ************************ */
GroundContactModel groundModel =
new LinearGroundContactModel(this, 1e3, 1e3, 5e3, 1e3, this.getRobotsYoVariableRegistry());
GroundProfile3D profile = new FlatGroundProfile();
groundModel.setGroundProfile3D(profile);
this.setGroundContactModel(groundModel);
}
/** Initialization for walking */
public void setInitialVelocity() {
qd_x = (DoubleYoVariable) getVariable("qd_x");
qd_x.set(0.8);
}
/** Inertias */
private Matrix3d createInertiaMatrixBox(LinkNames linkName) {
Matrix3d inertiaBox = new Matrix3d();
if (linkName == LinkNames.BODY_LINK) {
inertiaBox =
RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidBox(
cubeX, cubeY, cubeZ, bodyMass);
} else {
inertiaBox =
RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidBox(
footX, footY, footZ, footMass);
}
return inertiaBox;
}
private Matrix3d createInertiaMatrixCylinder(LinkNames linkName) {
Matrix3d inertiaCylinder = new Matrix3d();
inertiaCylinder =
RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidCylinder(
RobotParameters.MASSES.get(linkName),
RobotParameters.RADII.get(linkName),
RobotParameters.LENGTHS.get(linkName),
Axis.Z);
return inertiaCylinder;
}
/** Rigid Bodies and Links */
/** *********************** ID ROBOT - Rigid bodies ******************************* */
private void createAndAttachBodyRB(LinkNames linkName, SixDoFJoint bodyJointID) {
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
ScrewTools.addRigidBody(linkName.getName(), bodyJointID, inertiaBody, bodyMass, comOffsetBody);
}
private void createAndAttachCylinderRB(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Matrix3d inertiaCylinder = createInertiaMatrixCylinder(linkName);
Vector3d comOffsetCylinder =
new Vector3d(0.0, 0.0, -RobotParameters.LENGTHS.get(linkName) / 2.0);
allLegRigidBodies
.get(robotSide)
.put(
linkName,
ScrewTools.addRigidBody(
linkName.getName(),
allLegJoints.get(robotSide).get(jointName),
inertiaCylinder,
bodyMass,
comOffsetCylinder));
}
private void createAndAttachFootRB(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
ScrewTools.addRigidBody(
linkName.getName(),
allLegJoints.get(robotSide).get(JointNames.KNEE),
inertiaBody,
footMass,
comOffsetFoot);
}
/** *********************** SCS ROBOT - Links ******************************* */
private void createAndAttachBodyLink(LinkNames linkName) {
Link link = new Link(LinkNames.BODY_LINK.getName());
Matrix3d inertiaBody = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaBody);
link.setMass(RobotParameters.MASSES.get(LinkNames.BODY_LINK));
link.setComOffset(comOffsetBody);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCube(cubeX, cubeY, cubeZ, RobotParameters.APPEARANCE.get(LinkNames.BODY_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.7);
bodyJointSCS.setLink(link);
}
private void createAndAttachCylinderLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(linkName.getName());
Matrix3d inertiaCylinder = createInertiaMatrixCylinder(linkName);
link.setMomentOfInertia(inertiaCylinder);
link.setMass(RobotParameters.MASSES.get(linkName));
link.setComOffset(new Vector3d(0.0, 0.0, -RobotParameters.LENGTHS.get(linkName) / 2.0));
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.addCylinder(
-RobotParameters.LENGTHS.get(linkName),
RobotParameters.RADII.get(linkName),
RobotParameters.APPEARANCE.get(linkName));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
private void createAndAttachFootLink(
LinkNames linkName, JointNames jointName, RobotSide robotSide) {
Link link = new Link(LinkNames.FOOT_LINK.getName());
Matrix3d inertiaFoot = createInertiaMatrixBox(linkName);
link.setMomentOfInertia(inertiaFoot);
link.setMass(RobotParameters.MASSES.get(LinkNames.FOOT_LINK));
link.setComOffset(comOffsetFoot);
Graphics3DObject linkGraphics = new Graphics3DObject();
linkGraphics.translate(footOffsetX, 0.0, 0.0);
linkGraphics.addCube(footX, footY, -footZ, RobotParameters.APPEARANCE.get(LinkNames.FOOT_LINK));
link.setLinkGraphics(linkGraphics);
// link.addEllipsoidFromMassProperties(YoAppearance.Green());
link.addCoordinateSystemToCOM(0.3);
idToSCSLegJointMap.get(allLegJoints.get(robotSide).get(jointName)).setLink(link);
}
/** SCS Robot --> ID Robot Send positions and velocities. */
public void updatePositionsIDrobot() {
// Body info
bodyJointID.setPosition(
bodyJointSCS.getQ_t1().getDoubleValue(), 0.0, bodyJointSCS.getQ_t2().getDoubleValue());
bodyJointID.setRotation(0.0, bodyJointSCS.getQ_rot().getDoubleValue(), 0.0);
double[] velocityArray = new double[6];
velocityArray[0] = 0.0; // yaw
velocityArray[1] = bodyJointSCS.getQd_rot().getDoubleValue(); // pitch
velocityArray[2] = 0.0; // roll
velocityArray[3] = bodyJointSCS.getQd_t1().getDoubleValue(); // x
velocityArray[4] = 0.0; // y
velocityArray[5] = bodyJointSCS.getQd_t2().getDoubleValue(); // z
DenseMatrix64F velocities = new DenseMatrix64F(6, 1, true, velocityArray);
bodyJointID.setVelocity(velocities, 0);
// Leg and foot info (hip, knee and ankle)
for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
idJoint.setQ(scsJoint.getQYoVariable().getDoubleValue());
idJoint.setQd(scsJoint.getQDYoVariable().getDoubleValue());
}
elevator.updateFramesRecursively();
// Get the body coordinates
bodyPosition = new FramePoint();
bodyPosition.setToZero(bodyJointID.getFrameAfterJoint());
bodyPosition.changeFrame(worldFrame);
}
/** ID Robot --> SCS Robot Copy the torques from the IDRobot to the SCSRobot. */
public void updateTorquesSCSrobot() // Remember! the body joint is NOT actuated
{
for (OneDoFJoint idJoint : idToSCSLegJointMap.keySet()) {
OneDegreeOfFreedomJoint scsJoint = idToSCSLegJointMap.get(idJoint);
scsJoint.setTau(idJoint.getTau());
}
}
public RigidBody getElevator() {
return elevator;
}
/** Getters and Setters for the controller */
// ID joints and rigid bodies
public OneDoFJoint getLegJoint(JointNames jointName, RobotSide robotSide) {
return allLegJoints.get(robotSide).get(jointName);
}
public SixDoFJoint getBodyJoint() {
return bodyJointID;
}
public RigidBody getLegRigidBody(LinkNames linkName, RobotSide robotSide) {
return allLegRigidBodies.get(robotSide).get(linkName);
}
// Body
public double getBodyPositionZ() {
bodyZ = bodyPosition.getZ();
return bodyZ;
}
public double getBodyPositionX() {
bodyX = bodyPosition.getX();
return bodyX;
}
// public void getBodyPoint(Point3d bodyPointToPack) // Example of GET TO PACK method
// {
// Point3d bodyPoint = this.bodyPosition.getPoint();
// bodyPointToPack.set(bodyPoint);
// }
public void getBodyPitch(Quat4d rotationToPack) {
bodyJointID.getFrameAfterJoint().getRotation(rotationToPack);
}
public void getBodyLinearVel(Vector3d linearVelocityToPack) {
bodyJointID.getLinearVelocity(linearVelocityToPack);
}
public void getBodyAngularVel(Vector3d angularVelocityToPack) {
bodyJointID.getAngularVelocity(angularVelocityToPack);
}
public double getBodyVelX(Vector3d linearVelocityToPack) {
bodyJointID.getLinearVelocity(linearVelocityToPack);
double velX = linearVelocityToPack.getX();
return velX;
}
// Knee
public double getKneeVelocity(RobotSide robotSide) {
double kneeVelocity = allLegJoints.get(robotSide).get(JointNames.KNEE).getQd();
return kneeVelocity;
}
public void setKneeTau(RobotSide robotSide, double desiredKneeTau) {
allLegJoints.get(robotSide).get(JointNames.KNEE).setTau(desiredKneeTau);
}
public double getKneePosition(RobotSide robotSide) {
double kneeHeight = allLegJoints.get(robotSide).get(JointNames.KNEE).getQ();
return kneeHeight;
}
// Hip
public void setHipTau(RobotSide robotSide, double desiredHipTau) {
allLegJoints.get(robotSide).get(JointNames.HIP).setTau(desiredHipTau);
}
public double getHipVelocity(RobotSide robotSide) {
double hipVelocity = allLegJoints.get(robotSide).get(JointNames.HIP).getQd();
return hipVelocity;
}
public double getHipPitch(RobotSide robotSide) {
double hipPitch = allLegJoints.get(robotSide).get(JointNames.HIP).getQ();
return hipPitch;
}
// Ankle
public void setAnkleTau(RobotSide robotSide, double desiredAnkleTau) {
allLegJoints.get(robotSide).get(JointNames.ANKLE).setTau(desiredAnkleTau);
}
public double getAnkleVelocity(RobotSide robotSide) {
double ankleVelocity = allLegJoints.get(robotSide).get(JointNames.ANKLE).getQd();
return ankleVelocity;
}
public double getAnklePitch(RobotSide robotSide) {
double anklePitch = allLegJoints.get(robotSide).get(JointNames.ANKLE).getQ();
return anklePitch;
}
// Feet
public boolean heelOnTheFloor(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).isInContact();
}
public boolean heelToeOffAhead(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getX()
> GCpointsHeel.get(robotSide.getOppositeSide()).getX();
}
public boolean toeOnTheFloor(RobotSide robotSide) {
return GCpointsToe.get(robotSide).isInContact();
}
public void setFStoTrue(RobotSide robotSide) {
GCpointsHeel.get(robotSide).getYoFootSwitch().set(1.0);
}
public double getToeX(RobotSide robotSide) {
return GCpointsToe.get(robotSide).getX();
}
public double getHeelX(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getX();
}
public double getHeelZ(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getZ();
}
}
public double getHeelZ(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).getZ();
}
public double getToeX(RobotSide robotSide) {
return GCpointsToe.get(robotSide).getX();
}
public void setFStoTrue(RobotSide robotSide) {
GCpointsHeel.get(robotSide).getYoFootSwitch().set(1.0);
}
public boolean toeOnTheFloor(RobotSide robotSide) {
return GCpointsToe.get(robotSide).isInContact();
}
public double getGraspOffsetFromControlFrame(RobotSide robotSide) {
return graspOffsetFromCotnrolFrameAtomic.get(robotSide).get();
}
public double getDesiredAbsoluteSteeringAngle(RobotSide robotSide) {
return desiredAbsoluteSteeringAngle.get(robotSide).get();
}
/** Joints */
public Step6IDandSCSRobot_pinKnee() {
super("Robot");
/** **************** ID ROBOT ********************** */
elevator = new RigidBody("elevator", elevatorFrame);
bodyJointID = new SixDoFJoint(JointNames.BODY.getName(), elevator, elevatorFrame);
createAndAttachBodyRB(LinkNames.BODY_LINK, bodyJointID);
for (RobotSide robotSide : RobotSide.values) {
// HIP ID (location, joint, and rigidBody)
Vector3d hipOffset = new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0);
RevoluteJoint hipJointID =
ScrewTools.addRevoluteJoint(
JointNames.HIP.getName(),
bodyJointID.getSuccessor(),
hipOffset,
jointAxesPinJoints); // The parent rigid body of the hip joint is:
// bodyJointID.getSuccessor()
allLegJoints.get(robotSide).put(JointNames.HIP, hipJointID);
createAndAttachCylinderRB(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);
// KNEE ID
Vector3d kneeOffset = new Vector3d(0.0, 0.0, kneeOffsetZ);
RevoluteJoint kneeJointID =
ScrewTools.addRevoluteJoint(
JointNames.KNEE.getName(), hipJointID.getSuccessor(), kneeOffset, jointAxesPinJoints);
allLegJoints.get(robotSide).put(JointNames.KNEE, kneeJointID);
createAndAttachCylinderRB(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);
// ANKLE ID (location, joint, and rigidBody)
Vector3d ankleOffset = new Vector3d(0.0, 0.0, ankleOffsetZ);
RevoluteJoint ankleJointID =
ScrewTools.addRevoluteJoint(
JointNames.ANKLE.getName(),
kneeJointID.getSuccessor(),
ankleOffset,
jointAxesPinJoints);
allLegJoints.get(robotSide).put(JointNames.ANKLE, ankleJointID);
createAndAttachFootRB(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
}
/** **************** SCS ROBOT ********************** */
// BODY SCS
bodyJointSCS = new FloatingPlanarJoint(JointNames.BODY.getName(), this);
this.addRootJoint(bodyJointSCS);
createAndAttachBodyLink(LinkNames.BODY_LINK);
bodyJointSCS.setCartesianPosition(0.0, initialBodyHeight);
for (RobotSide robotSide : RobotSide.values) {
// HIP SCS
PinJoint hipJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.HIP.getName(),
new Vector3d(0.0, robotSide.negateIfRightSide(hipOffsetY), 0.0),
this,
jointAxesPinJoints);
hipJointSCS.setLimitStops(-0.75, 0.75, 1e3, 1e1);
// It is NOT necessary to set limits in the ID description because if the SCS description
// doesn't let the robot move passed a point the ID robot won't be able to pass it either
if (robotSide == RobotSide.LEFT) {
hipJointSCS.setQ(-0.6);
} else {
hipJointSCS.setQ(0.1);
}
bodyJointSCS.addJoint(hipJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.HIP), hipJointSCS);
createAndAttachCylinderLink(LinkNames.UPPER_LINK, JointNames.HIP, robotSide);
// KNEE SCS
PinJoint kneeJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.KNEE.getName(),
new Vector3d(0.0, 0.0, kneeOffsetZ),
this,
jointAxesPinJoints);
kneeJointSCS.setLimitStops(-0.01, 1.8, 1e5, 1e3);
hipJointSCS.addJoint(kneeJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.KNEE), kneeJointSCS);
createAndAttachCylinderLink(LinkNames.LOWER_LINK, JointNames.KNEE, robotSide);
// ANKLE SCS
PinJoint ankleJointSCS =
new PinJoint(
robotSide.getSideNameFirstLetter() + JointNames.ANKLE.getName(),
new Vector3d(0.0, 0.0, ankleOffsetZ),
this,
jointAxesPinJoints);
ankleJointSCS.setLimitStops(-0.7, 0.7, 1e3, 1e2);
if (robotSide == RobotSide.RIGHT) {
ankleJointSCS.setQ(-0.1);
}
kneeJointSCS.addJoint(ankleJointSCS);
idToSCSLegJointMap.put(allLegJoints.get(robotSide).get(JointNames.ANKLE), ankleJointSCS);
// FEET SCS
createAndAttachFootLink(LinkNames.FOOT_LINK, JointNames.ANKLE, robotSide);
GroundContactPoint gcHeel =
new GroundContactPoint(
robotSide.getSideNameFirstLetter() + "gcHeel",
new Vector3d(-0.1, 0.0, gcOffset),
this);
GCpointsHeel.set(robotSide, gcHeel);
ankleJointSCS.addGroundContactPoint(gcHeel);
Graphics3DObject graphicsGCheel = ankleJointSCS.getLink().getLinkGraphics();
graphicsGCheel.identity();
graphicsGCheel.translate(-0.1, 0.0, gcOffset);
graphicsGCheel.addSphere(gcRadius, YoAppearance.Orange());
if (robotSide == RobotSide.RIGHT) {
setFStoTrue(robotSide);
}
GroundContactPoint gcToe =
new GroundContactPoint(
robotSide.getSideNameFirstLetter() + "gcToe", new Vector3d(0.4, 0.0, gcOffset), this);
GCpointsToe.set(robotSide, gcToe);
ankleJointSCS.addGroundContactPoint(gcToe);
Graphics3DObject graphics = ankleJointSCS.getLink().getLinkGraphics();
graphics.identity();
graphics.translate(0.4, 0.0, gcOffset);
graphics.addSphere(gcRadius, YoAppearance.DarkOliveGreen());
}
/** ************** SCS Ground Model ************************ */
GroundContactModel groundModel =
new LinearGroundContactModel(this, 1e3, 1e3, 5e3, 1e3, this.getRobotsYoVariableRegistry());
GroundProfile3D profile = new FlatGroundProfile();
groundModel.setGroundProfile3D(profile);
this.setGroundContactModel(groundModel);
}
private SideDependentList<FootSwitchInterface> createFootSwitches(
SideDependentList<? extends ContactablePlaneBody> bipedFeet,
ForceSensorDataHolderReadOnly forceSensorDataHolder,
ContactSensorHolder contactSensorHolder,
double totalRobotWeight,
YoGraphicsListRegistry yoGraphicsListRegistry,
YoVariableRegistry registry) {
SideDependentList<FootSwitchInterface> footSwitches =
new SideDependentList<FootSwitchInterface>();
for (RobotSide robotSide : RobotSide.values) {
FootSwitchInterface footSwitch = null;
String footName = bipedFeet.get(robotSide).getName();
ForceSensorDataReadOnly footForceSensor =
forceSensorDataHolder.getByName(footSensorNames.get(robotSide));
double contactThresholdForce = walkingControllerParameters.getContactThresholdForce();
double footSwitchCoPThresholdFraction = walkingControllerParameters.getCoPThresholdFraction();
switch (walkingControllerParameters.getFootSwitchType()) {
case KinematicBased:
footSwitch =
new KinematicsBasedFootSwitch(
footName,
bipedFeet,
walkingControllerParameters.getContactThresholdHeight(),
totalRobotWeight,
robotSide,
registry); // controller switch doesnt need com
break;
case WrenchBased:
WrenchBasedFootSwitch wrenchBasedFootSwitch =
new WrenchBasedFootSwitch(
footName,
footForceSensor,
footSwitchCoPThresholdFraction,
totalRobotWeight,
bipedFeet.get(robotSide),
yoGraphicsListRegistry,
contactThresholdForce,
registry);
wrenchBasedFootSwitch.setSecondContactThresholdForce(
walkingControllerParameters.getSecondContactThresholdForceIgnoringCoP());
footSwitch = wrenchBasedFootSwitch;
break;
case WrenchAndContactSensorFused:
footSwitch =
new WrenchAndContactSensorFusedFootSwitch(
footName,
footForceSensor,
contactSensorHolder.getByName(footContactSensorNames.get(robotSide)),
footSwitchCoPThresholdFraction,
totalRobotWeight,
bipedFeet.get(robotSide),
yoGraphicsListRegistry,
contactThresholdForce,
registry);
break;
}
assert footSwitch != null;
footSwitches.put(robotSide, footSwitch);
}
return footSwitches;
}
// Feet
public boolean heelOnTheFloor(RobotSide robotSide) {
return GCpointsHeel.get(robotSide).isInContact();
}
@Override
public void initializePhysicalSensors(URI rosCoreURI) {
if (rosCoreURI == null)
throw new RuntimeException(
getClass().getSimpleName()
+ " Physical sensor requires rosURI to be set in "
+ NetworkParameters.defaultParameterFile);
sensorSuitePacketCommunicator.attachListener(
RobotConfigurationData.class, robotConfigurationDataBuffer);
ForceSensorNoiseEstimator forceSensorNoiseEstimator =
new ForceSensorNoiseEstimator(sensorSuitePacketCommunicator);
sensorSuitePacketCommunicator.attachListener(
RobotConfigurationData.class, forceSensorNoiseEstimator);
RosMainNode rosMainNode = new RosMainNode(rosCoreURI, "atlas/sensorSuiteManager", true);
DRCRobotCameraParameters multisenseLeftEyeCameraParameters =
sensorInformation.getCameraParameters(AtlasSensorInformation.MULTISENSE_SL_LEFT_CAMERA_ID);
DRCRobotLidarParameters multisenseLidarParameters =
sensorInformation.getLidarParameters(AtlasSensorInformation.MULTISENSE_LIDAR_ID);
DRCRobotPointCloudParameters multisenseStereoParameters =
sensorInformation.getPointCloudParameters(AtlasSensorInformation.MULTISENSE_STEREO_ID);
MultiSenseSensorManager multiSenseSensorManager =
new MultiSenseSensorManager(
modelFactory,
pointCloudDataReceiver,
robotConfigurationDataBuffer,
rosMainNode,
sensorSuitePacketCommunicator,
ppsTimestampOffsetProvider,
rosCoreURI,
multisenseLeftEyeCameraParameters,
multisenseLidarParameters,
multisenseStereoParameters,
sensorInformation.setupROSParameterSetters());
DRCRobotCameraParameters leftFishEyeCameraParameters =
sensorInformation.getCameraParameters(AtlasSensorInformation.BLACKFLY_LEFT_CAMERA_ID);
DRCRobotCameraParameters rightFishEyeCameraParameters =
sensorInformation.getCameraParameters(AtlasSensorInformation.BLACKFLY_RIGHT_CAMERA_ID);
FisheyeCameraReceiver leftFishEyeCameraReceiver =
new FisheyeCameraReceiver(
modelFactory,
leftFishEyeCameraParameters,
robotConfigurationDataBuffer,
sensorSuitePacketCommunicator,
ppsTimestampOffsetProvider,
rosMainNode);
FisheyeCameraReceiver rightFishEyeCameraReceiver =
new FisheyeCameraReceiver(
modelFactory,
rightFishEyeCameraParameters,
robotConfigurationDataBuffer,
sensorSuitePacketCommunicator,
ppsTimestampOffsetProvider,
rosMainNode);
leftFishEyeCameraReceiver.start();
rightFishEyeCameraReceiver.start();
VisionPoseEstimator visionPoseEstimator =
new VisionPoseEstimator(
sensorSuitePacketCommunicator,
pointCloudDataReceiver,
modelFactory,
robotConfigurationDataBuffer,
true);
multiSenseSensorManager.registerCameraListener(visionPoseEstimator);
blackFlyParameterSetters = new SideDependentList<BlackFlyParameterSetter>();
for (RobotSide side : RobotSide.values)
blackFlyParameterSetters.put(
side,
new BlackFlyParameterSetter(
rosMainNode,
side,
"/" + side.getLowerCaseName() + "/camera/camera_nodelet",
sensorSuitePacketCommunicator));
ppsTimestampOffsetProvider.attachToRosMainNode(rosMainNode);
// if (DRCConfigParameters.CALIBRATE_ARM_MODE)
// {
// ArmCalibrationHelper armCalibrationHelper = new
// ArmCalibrationHelper(sensorSuitePacketCommunicator, jointMap);
// multiSenseSensorManager.registerCameraListener(armCalibrationHelper);
// }
multiSenseSensorManager.initializeParameterListeners();
// IMUBasedHeadPoseCalculatorFactory.create(sensorSuitePacketCommunicator,
// sensorInformation, rosMainNode);
rosMainNode.execute();
}
@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;
}
}
}
}
