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); }
HandTrajectoryCommand command = new HandTrajectoryCommand(); command.set(message); controllerCommands.add(command);
HandTrajectoryCommand command = new HandTrajectoryCommand(); command.set(message); controllerCommands.add(command);
message.getSe3Trajectory().getFrameInformation().setTrajectoryReferenceFrameId(MessageTools.toFrameId(chestFrame)); message.getSe3Trajectory().getFrameInformation().setDataReferenceFrameId(MessageTools.toFrameId(worldFrame)); HandTrajectoryCommand command = new HandTrajectoryCommand(); command.getSE3Trajectory().set(worldFrame, chestFrame, message.getSe3Trajectory()); controllerCommands.add(command);
HandTrajectoryCommand handTrajectoryControllerCommand = new HandTrajectoryCommand(referenceFrame, userHandPoseSide.getEnumValue(), userHandPoseBaseForControl.getEnumValue());