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); }
private int queueExceedingTrajectoryPointsIfNeeded(FootTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); int maximumNumberOfWaypoints = positionTrajectoryGenerator.getMaximumNumberOfWaypoints() - positionTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; FootTrajectoryCommand commandForExcedent = commandQueue.addFirst(); numberOfQueuedCommands.increment(); commandForExcedent.clear(); commandForExcedent.setPropertiesOnly(command); for (int trajectoryPointIndex = maximumNumberOfWaypoints; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++) { commandForExcedent.addTrajectoryPoint(command.getTrajectoryPoint(trajectoryPointIndex)); } double timeOffsetToSubtract = command.getTrajectoryPoint(maximumNumberOfWaypoints - 1).getTime(); commandForExcedent.subtractTimeOffset(timeOffsetToSubtract); return maximumNumberOfWaypoints; }
public void handleFootTrajectoryCommand(List<FootTrajectoryCommand> commands) { for (int i = 0; i < commands.size(); i++) { FootTrajectoryCommand command = commands.get(i); upcomingFootTrajectoryCommandListForFlamingoStance.get(command.getRobotSide()).addLast(command); } }
FootTrajectoryCommand footTrajectoryCommand = new FootTrajectoryCommand(); 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); footTrajectoryCommand = new 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);
FootTrajectoryCommand command = new FootTrajectoryCommand(); command.set(message); 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); } } });
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);
RobotSide robotSide = command.getRobotSide(); FramePose desiredPose = new FramePose(); command.getLastTrajectoryPoint().getPoseIncludingFrame(desiredPose); desiredFootPoses.put(robotSide, desiredPose); DenseMatrix64F selectionMatrix = new DenseMatrix64F(command.getSelectionMatrix()); footSelectionMatrices.put(robotSide, selectionMatrix);
@Override public void set(FootTrajectoryCommand other) { se3Trajectory.set(other.se3Trajectory); setPropertiesOnly(other); }
@Override public void set(WholeBodyTrajectoryCommand other) { for (RobotSide robotSide : RobotSide.values) { handTrajectoryControllerCommands.get(robotSide).set(other.handTrajectoryControllerCommands.get(robotSide)); armTrajectoryControllerCommands.get(robotSide).set(other.armTrajectoryControllerCommands.get(robotSide)); footTrajectoryControllerCommands.get(robotSide).set(other.footTrajectoryControllerCommands.get(robotSide)); } chestTrajectoryControllerCommand.set(other.chestTrajectoryControllerCommand); pelvisTrajectoryControllerCommand.set(other.pelvisTrajectoryControllerCommand); }
@Override public void clear() { for (RobotSide robotSide : RobotSide.values) { handTrajectoryControllerCommands.get(robotSide).clear(); armTrajectoryControllerCommands.get(robotSide).clear(); footTrajectoryControllerCommands.get(robotSide).clear(); } chestTrajectoryControllerCommand.clear(); pelvisTrajectoryControllerCommand.clear(); }
FootTrajectoryCommand liftFootCommand = new FootTrajectoryCommand(); liftFootCommand.setRobotSide(robotSide); 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);
FootTrajectoryCommand command = new FootTrajectoryCommand(); command.set(message); controllerCommands.add(command);
@Override public void set(FootTrajectoryCommand other) { super.set(other); setPropertiesOnly(other); }
@Override public void set(WholeBodyTrajectoryMessage message) { clear(); for (RobotSide robotside : RobotSide.values) { HandTrajectoryMessage handTrajectoryMessage = message.getHandTrajectoryMessage(robotside); if (handTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) handTrajectoryControllerCommands.get(robotside).set(handTrajectoryMessage); ArmTrajectoryMessage armTrajectoryMessage = message.getArmTrajectoryMessage(robotside); if (armTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) armTrajectoryControllerCommands.get(robotside).set(armTrajectoryMessage); FootTrajectoryMessage footTrajectoryMessage = message.getFootTrajectoryMessage(robotside); if (footTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) footTrajectoryControllerCommands.get(robotside).set(footTrajectoryMessage); } ChestTrajectoryMessage chestTrajectoryMessage = message.getChestTrajectoryMessage(); if (chestTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) chestTrajectoryControllerCommand.set(chestTrajectoryMessage); PelvisTrajectoryMessage pelvisTrajectoryMessage = message.getPelvisTrajectoryMessage(); if (pelvisTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) pelvisTrajectoryControllerCommand.set(pelvisTrajectoryMessage); }
success = success && drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(9.0); FootTrajectoryCommand footTrajectoryCommand = new 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);
public void handleFootTrajectoryCommand(FootTrajectoryCommand command) { RobotSide robotSide = command.getRobotSide(); FootControlModule footControlModule = footControlModules.get(robotSide); footControlModule.handleFootTrajectoryCommand(command); if (footControlModule.getCurrentConstraintType() != ConstraintType.MOVE_VIA_WAYPOINTS) setContactStateForMoveViaWaypoints(robotSide); }