@Override public EuclideanTrajectoryPointMessage transform(RigidBodyTransform transform) { EuclideanTrajectoryPointMessage transformedTrajectoryPointMessage = new EuclideanTrajectoryPointMessage(); transformedTrajectoryPointMessage.time = time; if (position != null) transformedTrajectoryPointMessage.position = TransformTools.getTransformedPoint(position, transform); else transformedTrajectoryPointMessage.position = null; if (linearVelocity != null) transformedTrajectoryPointMessage.linearVelocity = TransformTools.getTransformedVector(linearVelocity, transform); else transformedTrajectoryPointMessage.linearVelocity = null; return transformedTrajectoryPointMessage; }
public FootstepDataMessage transform(RigidBodyTransform transform) { FootstepDataMessage ret = this.clone(); // Point3d location; ret.location = TransformTools.getTransformedPoint(this.getLocation(), transform); // Quat4d orientation; ret.orientation = TransformTools.getTransformedQuat(this.getOrientation(), transform); // Waypoints if they exist: if (trajectoryWaypoints != null) { for (int i = 0; i < trajectoryWaypoints.length; i++) ret.trajectoryWaypoints[i] = TransformTools.getTransformedPoint(trajectoryWaypoints[i], transform); } return ret; }
public static void main(String[] args) { RigidBodyTransform t = new RigidBodyTransform(); t.setTranslationAndIdentityRotation(new Vector3d(4.5, 6.6, 22)); System.out.println(getTransformedPoint(new Point3d(0.0, 1.0, 4.6), t)); }
public TorusPosePacket transform(RigidBodyTransform transform) { TorusPosePacket ret = new TorusPosePacket(); ret.fingerHoleRadius = this.fingerHoleRadius; ret.orientation = TransformTools.getTransformedQuat(this.getOrientation(), transform); ret.position = TransformTools.getTransformedPoint(this.getPosition(), transform); return ret; }
public SpigotPosePacket transform(RigidBodyTransform transform) { SpigotPosePacket ret = new SpigotPosePacket(); ret.orientation = TransformTools.getTransformedQuat(this.getOrientation(), transform); ret.position = TransformTools.getTransformedPoint(this.getPosition(), transform); return ret; }
public AdjustFootstepMessage transform(RigidBodyTransform transform) { AdjustFootstepMessage ret = this.clone(); // Point3d location; ret.location = TransformTools.getTransformedPoint(this.getLocation(), transform); // Quat4d orientation; ret.orientation = TransformTools.getTransformedQuat(this.getOrientation(), transform); return ret; }
public VehiclePosePacket transform(RigidBodyTransform transform) { VehiclePosePacket ret = new VehiclePosePacket(); // Point3d position; ret.position = TransformTools.getTransformedPoint(this.getPosition(), transform); // Quat4d orientation; ret.orientation = TransformTools.getTransformedQuat(this.getOrientation(), transform); return ret; }
@Override public VideoPacket transform(RigidBodyTransform transform) { Point3d newPoint = TransformTools.getTransformedPoint(getPosition(), transform); Quat4d newOrientation = TransformTools.getTransformedQuat(getOrientation(), transform); return new VideoPacket(getVideoSource(), getTimeStamp(), getData(), newPoint, newOrientation, getIntrinsicParameters()); }
public HandstepPacket transform(RigidBodyTransform transform) { HandstepPacket ret = new HandstepPacket(); // String rigidBodyName; ret.robotSide = this.getRobotSide(); // Point3d location; ret.location = TransformTools.getTransformedPoint(this.getLocation(), transform); // Quat4d orientation; ret.orientation = TransformTools.getTransformedQuat(this.getOrientation(), transform); // Vector3d surface normal ret.surfaceNormal = TransformTools.getTransformedVector(this.getSurfaceNormal(), transform); ret.swingTrajectoryTime = this.getSwingTrajectoryTime(); return ret; }