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