public SO3TrajectoryPointMessage transform(RigidBodyTransform transform) { SO3TrajectoryPointMessage transformedTrajectoryPointMessage = new SO3TrajectoryPointMessage(); transformedTrajectoryPointMessage.time = time; if (orientation != null) transformedTrajectoryPointMessage.orientation = TransformTools.getTransformedQuat(orientation, transform); else transformedTrajectoryPointMessage.orientation = null; if (angularVelocity != null) transformedTrajectoryPointMessage.angularVelocity = TransformTools.getTransformedVector(angularVelocity, transform); else transformedTrajectoryPointMessage.angularVelocity = null; return transformedTrajectoryPointMessage; }
@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 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; }