public void holdConfiguration(RigidBodyBasics rigidBody) { if (hasTrajectoryCommand) { initialGuessHolder.set(initialGuessCandidate); } else { initialGuessHolder.set(rigidBody.getBodyFixedFrame().getTransformToWorldFrame()); } }
@Override protected void setBehaviorInput() { RobotSide initialStanceSide = RobotSide.LEFT; RigidBodyTransform soleToWorld = referenceFrames.getSoleFrame(initialStanceSide).getTransformToWorldFrame(); FramePose3D stanceFootPose = new FramePose3D(ReferenceFrame.getWorldFrame(), soleToWorld); if (goalPose == null) System.err.println("WalkToLocationPlannedBehavior: goal pose NULL"); planPathToLocationBehavior.setInputs(goalPose, stanceFootPose, initialStanceSide); publishTextToSpeack("Planning Path"); } };
public ArrayList<Double> getFootstepLengths(FootstepDataListMessage footStepList, FullHumanoidRobotModel fullRobotModel, WalkingControllerParameters walkingControllerParameters) { ArrayList<Double> footStepLengths = new ArrayList<Double>(); List<FootstepDataMessage> dataList = footStepList.getFootstepDataList(); for (int i = 0; i < dataList.size(); i++) { FootstepDataMessage step = dataList.get(i); footstepDataList.add(step); } FootstepDataMessage firstStepData = footstepDataList.remove(footstepDataList.size() - 1); RigidBodyTransform firstSingleSupportFootTransformToWorld = fullRobotModel.getFoot(RobotSide.fromByte(firstStepData.getRobotSide()).getOppositeSide()) .getBodyFixedFrame().getTransformToWorldFrame(); firstSingleSupportFootTransformToWorld.getTranslation(firstSingleSupportFootTranslationFromWorld); previousFootStepLocation.set(firstSingleSupportFootTranslationFromWorld); nextFootStepLocation.set(firstStepData.getLocation()); while (!footstepDataList.isEmpty()) { footStepLengths.add(previousFootStepLocation.distance(nextFootStepLocation)); previousFootStepLocation.set(nextFootStepLocation); nextFootStepLocation.set(footstepDataList.remove(footstepDataList.size() - 1).getLocation()); } double lastStepLength = previousFootStepLocation.distance(nextFootStepLocation); footStepLengths.add(lastStepLength); return footStepLengths; }
public static KinematicsPlanningToolboxRigidBodyMessage holdRigidBodyCurrentPose(RigidBodyBasics rigidBody, TDoubleArrayList keyFrameTimes) { List<Pose3DReadOnly> currentPoses = new ArrayList<Pose3DReadOnly>(); for (int i = 0; i < keyFrameTimes.size(); i++) currentPoses.add(new Pose3D(rigidBody.getBodyFixedFrame().getTransformToWorldFrame())); KinematicsPlanningToolboxRigidBodyMessage message = HumanoidMessageTools.createKinematicsPlanningToolboxRigidBodyMessage(rigidBody, keyFrameTimes, currentPoses); message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(20.0)); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(20.0)); return message; } }
@Override public void onBehaviorEntered() { System.out.println("kinematics planning behavior"); if (keyFrameTimes.size() == 0) throw new RuntimeException("key frame times should be set ahead."); if (rigidBodyMessages.size() == 0) throw new RuntimeException("rigid body key frames should be set ahead."); toolboxStatePublisher.publish(MessageTools.createToolboxStateMessage(ToolboxState.WAKE_UP)); for (int i = 0; i < rigidBodyMessages.size(); i++) { KinematicsPlanningToolboxRigidBodyMessage message = rigidBodyMessages.get(i); message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(defaultRigidBodyWeight)); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(defaultRigidBodyWeight)); rigidBodyMessagePublisher.publish(message); } Vector3DReadOnly translationVector = fullRobotModel.getRootBody().getBodyFixedFrame().getTransformToWorldFrame().getTranslationVector(); List<Point3DReadOnly> desiredCOMPoints = new ArrayList<Point3DReadOnly>(); for (int i = 0; i < keyFrameTimes.size(); i++) desiredCOMPoints.add(new Point3D(translationVector)); KinematicsPlanningToolboxCenterOfMassMessage comMessage = HumanoidMessageTools.createKinematicsPlanningToolboxCenterOfMassMessage(keyFrameTimes, desiredCOMPoints); SelectionMatrix3D selectionMatrix = new SelectionMatrix3D(); selectionMatrix.selectZAxis(false); comMessage.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix)); comMessage.getWeights().set(MessageTools.createWeightMatrix3DMessage(defaultCOMWeight)); comMessagePublisher.publish(comMessage); System.out.println("published"); }
humanoidReferenceFrames.getChestFrame().getTransformToWorldFrame().getTranslation(chestPosition);
RigidBodyTransform handTransform = handControlFrame.getTransformToWorldFrame();
public ReachabilitySphereMapCalculator(OneDoFJointBasics[] robotArmJoints, SimulationConstructionSet scs) { this.scs = scs; solver = new ReachabilityMapSolver(robotArmJoints, null, registry); FramePose3D gridFramePose = new FramePose3D(ReferenceFrame.getWorldFrame(), robotArmJoints[0].getFrameBeforeJoint().getTransformToWorldFrame()); gridFramePose.appendTranslation(getGridSizeInMeters() / 2.5, 0.0, 0.0); setGridFramePose(gridFramePose); scs.addStaticLinkGraphics(ReachabilityMapTools.createReachibilityColorScale()); scs.addYoGraphic(gridFrameViz); scs.addYoGraphic(currentEvaluationPose); scs.addYoGraphic(currentEvaluationPosition); scs.addYoVariableRegistry(registry); }
public void testSingleWaypointInUserMode() throws Exception { BambooTools.reportTestStartedMessage(simulationTestingParameters.getShowWindows()); Random random = new Random(564574L); DRCObstacleCourseStartingLocation selectedLocation = DRCObstacleCourseStartingLocation.DEFAULT; drcSimulationTestHelper = new DRCSimulationTestHelper(simulationTestingParameters, getRobotModel()); drcSimulationTestHelper.setStartingLocation(selectedLocation); drcSimulationTestHelper.createSimulation(getClass().getSimpleName()); ThreadTools.sleep(1000); boolean success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(0.5); assertTrue(success); FullHumanoidRobotModel fullRobotModel = drcSimulationTestHelper.getControllerFullRobotModel(); double trajectoryTime = 1.0; RigidBodyBasics pelvis = fullRobotModel.getPelvis(); FramePoint3D desiredRandomPelvisPosition = getRandomPelvisPosition(random, pelvis); Point3D desiredPosition = new Point3D(desiredRandomPelvisPosition); System.out.println(desiredPosition); desiredRandomPelvisPosition.changeFrame(ReferenceFrame.getWorldFrame()); desiredPosition.set(desiredRandomPelvisPosition); System.out.println(desiredPosition); PelvisHeightTrajectoryMessage pelvisHeightTrajectoryMessage = HumanoidMessageTools.createPelvisHeightTrajectoryMessage(trajectoryTime, desiredPosition.getZ()); pelvisHeightTrajectoryMessage.setEnableUserPelvisControl(true); drcSimulationTestHelper.publishToController(pelvisHeightTrajectoryMessage); success = drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(1.0 + trajectoryTime); assertTrue(success); double pelvisHeight = fullRobotModel.getPelvis().getParentJoint().getFrameAfterJoint().getTransformToWorldFrame().getTranslationZ(); assertEquals(desiredPosition.getZ(), pelvisHeight, 0.01); drcSimulationTestHelper.createVideo(getSimpleRobotName(), 2); }
Pose3D originOfRigidBody = new Pose3D(rigidBody.getBodyFixedFrame().getTransformToWorldFrame()); waypoints.add(originOfRigidBody); waypoints.add(originOfRigidBody);
CommonHumanoidReferenceFrames referenceFrames = drcSimulationTestHelper.getReferenceFrames(); referenceFrames.updateFrames(); double initialPelvisHeight = referenceFrames.getPelvisFrame().getTransformToWorldFrame().getTranslationZ(); double finalPelvisHeight = referenceFrames.getPelvisFrame().getTransformToWorldFrame().getTranslationZ(); Assert.assertEquals(initialPelvisHeight, finalPelvisHeight, 1.0e-5);
sdfFullRobotModel.getHand(robotSide).getBodyFixedFrame().getTransformToWorldFrame()); for (int i = 0; i < numberOfKeyFrames; i++) Pose3D finalPose = new Pose3D(sdfFullRobotModel.getHand(robotSide).getBodyFixedFrame().getTransformToWorldFrame());
public static FootTrajectoryMessage createPutDownFootMessage(RobotSide side, HumanoidReferenceFrames referenceFrames) { FootTrajectoryMessage message = new FootTrajectoryMessage(); message.setRobotSide(side.toByte()); FramePose3D footPose = new FramePose3D(referenceFrames.getSoleFrame(side)); footPose.changeFrame(referenceFrames.getSoleFrame(side.getOppositeSide())); footPose.setX(0.0); footPose.changeFrame(ReferenceFrame.getWorldFrame()); double z = referenceFrames.getSoleFrame(side.getOppositeSide()).getTransformToWorldFrame().getTranslationZ(); footPose.setZ(z); SE3TrajectoryPointMessage trajectoryPoint = message.getSe3Trajectory().getTaskspaceTrajectoryPoints().add(); trajectoryPoint.getPosition().set(footPose.getPosition()); trajectoryPoint.getOrientation().set(footPose.getOrientation()); trajectoryPoint.setTime(0.1); return message; }
public static FootTrajectoryMessage createPickUpFootMessage(RobotSide side, HumanoidReferenceFrames referenceFrames) { FootTrajectoryMessage message = new FootTrajectoryMessage(); message.setRobotSide(side.toByte()); FramePose3D footPose = new FramePose3D(referenceFrames.getSoleFrame(side)); footPose.changeFrame(referenceFrames.getSoleFrame(side.getOppositeSide())); footPose.setX(0.0); footPose.changeFrame(ReferenceFrame.getWorldFrame()); double z = referenceFrames.getSoleFrame(side.getOppositeSide()).getTransformToWorldFrame().getTranslationZ(); footPose.setZ(z + 0.05); SE3TrajectoryPointMessage trajectoryPoint = message.getSe3Trajectory().getTaskspaceTrajectoryPoints().add(); trajectoryPoint.getPosition().set(footPose.getPosition()); trajectoryPoint.getOrientation().set(footPose.getOrientation()); trajectoryPoint.setTime(0.1); return message; }
RigidBodyTransform solutionRigidBodyTransform = rigidBodyOfOutputFullRobotModel.getBodyFixedFrame().getTransformToWorldFrame(); Pose3D solutionRigidBodyPose = new Pose3D(solutionRigidBodyTransform);
public static FootstepDataListMessage createStepsInPlace(HumanoidReferenceFrames referenceFrames) { FootstepDataListMessage message = new FootstepDataListMessage(); double z = referenceFrames.getSoleFrame(RobotSide.LEFT).getTransformToWorldFrame().getTranslationZ(); for (RobotSide side : RobotSide.values) { FootstepDataMessage step = message.getFootstepDataList().add(); step.setRobotSide(side.toByte()); FramePose3D footPose = new FramePose3D(referenceFrames.getSoleFrame(side)); footPose.changeFrame(referenceFrames.getSoleFrame(RobotSide.LEFT)); footPose.setX(0.1); footPose.changeFrame(ReferenceFrame.getWorldFrame()); double yaw = footPose.getYaw(); footPose.setOrientationYawPitchRoll(yaw, 0.0, 0.0); footPose.setZ(z); step.getLocation().set(footPose.getPosition()); step.getOrientation().set(footPose.getOrientation()); step.setSwingDuration(0.2); step.setTransferDuration(0.2); } message.setFinalTransferDuration(0.2); return message; }
public void updateRighdBodyTransform() RigidBodyTransform currentTransform = currentJoint.getFrameBeforeJoint().getTransformToWorldFrame(); RigidBodyTransform nextTransform = nextJoint.getFrameBeforeJoint().getTransformToWorldFrame(); currentTransform.getTranslation(currentLocation); nextTransform.getTranslation(nextLocation);
ReferenceFrame supportFootFrame = randomizedFullRobotModel.getFoot(supportFootSide).getBodyFixedFrame(); RigidBodyTransform transformFromRootJointToWorldFrame = rootJointFrame.getTransformToDesiredFrame(supportFootFrame); RigidBodyTransform initialSupportFootTransform = initialFullRobotModel.getFoot(supportFootSide).getBodyFixedFrame().getTransformToWorldFrame(); transformFromRootJointToWorldFrame.preMultiply(initialSupportFootTransform);
handPoses.put(robotSide, handPose); RigidBodyBasics hand = desiredFullRobotModel.getHand(robotSide); RigidBodyTransform transformToWorldFrame = hand.getBodyFixedFrame().getTransformToWorldFrame(); handPose.set(transformToWorldFrame);
private void handleIcpOffsetSending() lastPelvisPoseInWorldFrame.set(fullRobotModel.getPelvis().getBodyFixedFrame().getTransformToWorldFrame()); stateEstimatorPelvisPoseBuffer.put(lastPelvisPoseInWorldFrame, Conversions.secondsToNanoseconds(yoTime.getDoubleValue()));