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); }
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.setLocation(new Point3d(GenericROSTranslationTools.convertVector3(message.getLocation()))); ihmcMessage.setOrientation(new Quat4d(GenericROSTranslationTools.convertQuaternion(message.getOrientation()))); ihmcMessage.setSwingHeight(message.getSwingHeight());
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());