message.getSe3Trajectory().getFrameInformation().setDataReferenceFrameId(MessageTools.toFrameId(worldFrame)); FootTrajectoryCommand command = new FootTrajectoryCommand(); command.getSE3Trajectory().set(worldFrame, worldFrame, message.getSe3Trajectory()); controllerCommands.add(command);
footPointList.addTrajectoryPoint(4.0, new Point3D(0.0, -0.2, 0.15), new Quaternion(0.0, 0.0, 0.0, 1.0), new Vector3D(), new Vector3D()); footTrajectoryCommand.getSE3Trajectory().setTrajectoryPointList(footPointList); footTrajectoryCommand.setRobotSide(RobotSide.RIGHT); queuedControllerCommands.add(footTrajectoryCommand); footPointList.addTrajectoryPoint(4.0, new Point3D(0.0, -0.2, 0.15), new Quaternion(0.0, 0.1, 0.0, 1.0), new Vector3D(), new Vector3D()); footTrajectoryCommand.getSE3Trajectory().setTrajectoryPointList(footPointList); footTrajectoryCommand.setRobotSide(RobotSide.RIGHT); queuedControllerCommands.add(footTrajectoryCommand);
footPointList.addTrajectoryPoint(2.0, new Point3D(1.1, -0.2, 0.35), new Quaternion(0.0, 0.0, 0.0, 1.0), new Vector3D(), new Vector3D()); footTrajectoryCommand.getSE3Trajectory().setTrajectoryPointList(footPointList); footTrajectoryCommand.setRobotSide(RobotSide.RIGHT); footTrajectoryCommand.getSE3Trajectory().setTrajectoryFrame(ReferenceFrame.getWorldFrame()); queuedControllerCommands.add(footTrajectoryCommand);
liftFootCommand.getSE3Trajectory().addTrajectoryPoint(1.0, new Point3D(footPosition), new Quaternion(0.0, 0.0, 0.0, 1.0), new Vector3D(), new Vector3D()); queuedControllerCommands.add(liftFootCommand);