ihmcMessage.setOrigin(FootstepDataMessage.FootstepOrigin.values()[message.getOrigin()]); ihmcMessage.setRobotSide(RobotSide.values[message.getRobotSide()]); ihmcMessage.setLocation(new Point3d(GenericROSTranslationTools.convertVector3(message.getLocation()))); ihmcMessage.setOrientation(new Quat4d(GenericROSTranslationTools.convertQuaternion(message.getOrientation()))); ihmcMessage.setSwingHeight(message.getSwingHeight()); ihmcMessage.setTrajectoryType(TrajectoryType.values()[message.getTrajectoryType()]); ihmcMessage.setUniqueId(message.getUniqueId()); if (message.getHasTimings()) ihmcMessage.setTimings(message.getSwingTime(), message.getTransferTime()); if (message.getHasAbsoluteTime()) ihmcMessage.setAbsoluteTime(message.getSwingStartTime()); for (Point2dRosMessage point2dRosMessage : message.getPredictedContactPoints()) Point3d[] trajectoryWaypoints = new Point3d[message.getTrajectoryWaypoints().size()]; for (int i = 0; i < message.getTrajectoryWaypoints().size(); i++) trajectoryWaypoints[i] = new Point3d(GenericROSTranslationTools.convertVector3(message.getTrajectoryWaypoints().get(i)));
message.setUniqueId(footstep.getUniqueId()); message.setLocation(GenericROSTranslationTools.convertTuple3d(footstep.getLocation())); message.setOrientation(GenericROSTranslationTools.convertTuple4d(footstep.getOrientation())); message.setOrigin((byte) footstep.getOrigin().ordinal()); message.setRobotSide((byte) footstep.getRobotSide().ordinal()); message.setSwingHeight(footstep.getSwingHeight()); message.setTrajectoryType((byte) footstep.getTrajectoryType().ordinal()); message.setSwingTime(footstep.swingTime); message.setTransferTime(footstep.transferTime); message.setSwingTime(Double.NaN); message.setTransferTime(Double.NaN); message.setHasTimings(footstep.hasTimings); message.setSwingStartTime(footstep.swingStartTime); message.setSwingStartTime(0.0); message.setHasAbsoluteTime(footstep.hasAbsoluteTime); message.setPredictedContactPoints(predictedContatcPointsRos); message.setTrajectoryWaypoints(trajectoryWaypoints);
private static Packet customConvertToIHMCMessage(FootstepDataRosMessage message) throws Exception { FootstepDataMessage ihmcMessage = new FootstepDataMessage(); ihmcMessage.setOrigin(FootstepDataMessage.FootstepOrigin.values()[message.getOrigin()]); ihmcMessage.setRobotSide(RobotSide.values[message.getRobotSide()]); ihmcMessage.setLocation(new Point3d(GenericROSTranslationTools.convertVector3(message.getLocation()))); ihmcMessage.setOrientation(new Quat4d(GenericROSTranslationTools.convertQuaternion(message.getOrientation()))); ihmcMessage.setSwingHeight(message.getSwingHeight()); ihmcMessage.setTrajectoryType(TrajectoryType.values()[message.getTrajectoryType()]); ihmcMessage.setUniqueId(message.getUniqueId()); ArrayList<Point2d> predictedContactPoints = new ArrayList<>(); for (Point2dRosMessage point2dRosMessage : message.getPredictedContactPoints()) { predictedContactPoints.add(GenericROSTranslationTools.convertPoint2DRos(point2dRosMessage)); } ihmcMessage.setPredictedContactPoints(predictedContactPoints); return ihmcMessage; }
ihmcMessage.setRobotSide(message.getRobotSide()); ihmcMessage.getLocation().set(new Point3D(GenericROSTranslationTools.convertPoint(message.getLocation()))); ihmcMessage.getOrientation().set(new us.ihmc.euclid.tuple4D.Quaternion(GenericROSTranslationTools.convertQuaternion(message.getOrientation()))); ihmcMessage.setSwingHeight(message.getSwingHeight()); ihmcMessage.setTrajectoryType(message.getTrajectoryType()); ihmcMessage.setUniqueId(message.getUniqueId()); ihmcMessage.setSwingDuration(message.getSwingDuration()); ihmcMessage.setTransferDuration(message.getTransferDuration()); ihmcMessage.setTouchdownDuration(message.getTouchdownDuration()); ihmcMessage.setSwingTrajectoryBlendDuration(message.getSwingTrajectoryBlendDuration()); for (Point2dRosMessage point2dRosMessage : message.getPredictedContactPoints()) Point3D[] trajectoryWaypoints = new Point3D[message.getCustomPositionWaypoints().size()]; for (int i = 0; i < message.getCustomPositionWaypoints().size(); i++) trajectoryWaypoints[i] = new Point3D(GenericROSTranslationTools.convertPoint(message.getCustomPositionWaypoints().get(i))); for (SE3TrajectoryPointRosMessage rosTrajectoryPoint : message.getSwingTrajectory())
message.setUniqueId(footstep.getUniqueId()); message.setLocation(GenericROSTranslationTools.convertPoint3D(footstep.getLocation())); message.setOrientation(GenericROSTranslationTools.convertTuple4d(footstep.getOrientation())); message.setRobotSide(footstep.getRobotSide()); message.setSwingHeight(footstep.getSwingHeight()); message.setTrajectoryType(footstep.getTrajectoryType()); message.setSwingDuration(footstep.getSwingDuration()); message.setTransferDuration(footstep.getTransferDuration()); message.setTouchdownDuration(footstep.getTouchdownDuration()); message.setSwingTrajectoryBlendDuration(footstep.getSwingTrajectoryBlendDuration()); message.setSwingTrajectory(rosSwingTrajectory); message.setPredictedContactPoints(predictedContatcPointsRos); message.setCustomPositionWaypoints(positionWaypoints);
private static Message customConvertToRosMessage(FootstepDataMessage footstep) throws Exception { Class<? extends Packet> ihmcMessageClass = FootstepDataMessage.class; String rosMessageClassNameFromIHMCMessage = GenericROSTranslationTools.getRosMessageClassNameFromIHMCMessage(ihmcMessageClass.getSimpleName()); RosMessagePacket rosAnnotation = ihmcMessageClass.getAnnotation(RosMessagePacket.class); FootstepDataRosMessage message = messageFactory.newFromType(rosAnnotation.rosPackage() + "/" + rosMessageClassNameFromIHMCMessage); message.setUniqueId(footstep.getUniqueId()); message.setLocation(GenericROSTranslationTools.convertTuple3d(footstep.getLocation())); message.setOrientation(GenericROSTranslationTools.convertTuple4d(footstep.getOrientation())); message.setOrigin((byte) footstep.getOrigin().ordinal()); message.setRobotSide((byte) footstep.getRobotSide().ordinal()); message.setSwingHeight(footstep.getSwingHeight()); message.setTrajectoryType((byte) footstep.getTrajectoryType().ordinal()); List<Point2dRosMessage> predictedContatcPointsRos = new ArrayList<>(); if (footstep.predictedContactPoints != null) { for (Point2d predictedContactPoint : footstep.predictedContactPoints) { predictedContatcPointsRos.add(GenericROSTranslationTools.convertPoint2d(predictedContactPoint)); } } message.setPredictedContactPoints(predictedContatcPointsRos); return message; }
public static us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataMessage convertFootStepData(FootstepDataRosMessage msg) { RobotSide robotSide = RobotSide.values[(int) msg.getRobotSide()]; Vector3 loc = msg.getLocation(); Quaternion quat = msg.getOrientation(); Point3d location = new Point3d(loc.getX(), loc.getY(), loc.getZ()); Quat4d orientation = new Quat4d(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); return new us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataMessage(robotSide, location, orientation); }
public static us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataMessage convertFootStepData(FootstepDataRosMessage msg) { RobotSide robotSide = RobotSide.values[(int) msg.getRobotSide()]; Vector3 loc = msg.getLocation(); Quaternion quat = msg.getOrientation(); Point3d location = new Point3d(loc.getX(), loc.getY(), loc.getZ()); Quat4d orientation = new Quat4d(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); return new us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataMessage(robotSide, location, orientation); }