public WholeBodyTrajectoryCommand() { for (RobotSide robotSide : RobotSide.values) { HandTrajectoryCommand handTrajectoryControllerCommand = new HandTrajectoryCommand(); ArmTrajectoryCommand armTrajectoryControllerCommand = new ArmTrajectoryCommand(); FootTrajectoryCommand footTrajectoryControllerCommand = new FootTrajectoryCommand(); handTrajectoryControllerCommands.put(robotSide, handTrajectoryControllerCommand); armTrajectoryControllerCommands.put(robotSide, armTrajectoryControllerCommand); footTrajectoryControllerCommands.put(robotSide, footTrajectoryControllerCommand); allControllerCommands.add(handTrajectoryControllerCommand); allControllerCommands.add(armTrajectoryControllerCommand); allControllerCommands.add(footTrajectoryControllerCommand); } allControllerCommands.add(chestTrajectoryControllerCommand); allControllerCommands.add(pelvisTrajectoryControllerCommand); }
FootTrajectoryCommand command = new FootTrajectoryCommand(); command.set(message); controllerCommands.add(command);
FootTrajectoryCommand command = new FootTrajectoryCommand(); command.set(message); controllerCommands.add(command);
message.getSe3Trajectory().getFrameInformation().setTrajectoryReferenceFrameId(MessageTools.toFrameId(worldFrame)); message.getSe3Trajectory().getFrameInformation().setDataReferenceFrameId(MessageTools.toFrameId(worldFrame)); FootTrajectoryCommand command = new FootTrajectoryCommand(); command.getSE3Trajectory().set(worldFrame, worldFrame, message.getSe3Trajectory()); controllerCommands.add(command);
public void variableChanged(YoVariable<?> v) { if (userDoFootPose.getBooleanValue()) { userDesiredFootPose.getFramePose(framePose); ReferenceFrame soleZUpFrame = ankleZUpFrames.get(userFootPoseSide.getEnumValue()); soleZUpFrame.update(); framePose.setIncludingFrame(soleZUpFrame, framePose.getGeometryObject()); framePose.changeFrame(ReferenceFrame.getWorldFrame()); System.out.println("framePose " + framePose); FootTrajectoryCommand footTrajectoryControllerCommand = new FootTrajectoryCommand(); FrameSE3TrajectoryPoint trajectoryPoint = new FrameSE3TrajectoryPoint(ReferenceFrame.getWorldFrame()); trajectoryPoint.setTime(userDesiredFootPoseTrajectoryTime.getDoubleValue()); trajectoryPoint.setPosition(framePose.getFramePointCopy()); trajectoryPoint.setOrientation(framePose.getFrameOrientationCopy()); trajectoryPoint.setLinearVelocity(new Vector3d()); trajectoryPoint.setAngularVelocity(new Vector3d()); footTrajectoryControllerCommand.addTrajectoryPoint(trajectoryPoint); footTrajectoryControllerCommand.setRobotSide(userFootPoseSide.getEnumValue()); System.out.println("Submitting " + footTrajectoryControllerCommand); controllerCommandInputManager.submitCommand(footTrajectoryControllerCommand); userDoFootPose.set(false); } } });
FootTrajectoryCommand footTrajectoryCommand = new FootTrajectoryCommand(); footTrajectoryCommand = new FootTrajectoryCommand();
FootTrajectoryCommand liftFootCommand = new FootTrajectoryCommand(); liftFootCommand.setRobotSide(robotSide);
success = success && drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(9.0); FootTrajectoryCommand footTrajectoryCommand = new FootTrajectoryCommand();