private boolean isTransformationInPlane(RigidBodyTransform transform) { // arguably not a sufficient condition. Reference frames could just happen to be aligned (not likely though). ReferenceFrame2d needed! double[] transformArray = new double[16]; transform.get(transformArray); if (((transformArray[2] == 0.0) && (transformArray[6] == 0.0) && (transformArray[8] == 0.0) && (transformArray[9] == 0.0) && (transformArray[10] == 1.0))) { return true; } else { return false; } }
public static double getMaxLInfiniteDistance(RigidBodyTransform t1, RigidBodyTransform t2) { double[] t1Matrix = new double[16]; t1.get(t1Matrix); double[] t2Matrix = new double[16]; t2.get(t2Matrix); double maxDifference = 0.0; for (int i = 0; i < 16; i++) { double difference = t1Matrix[i] - t2Matrix[i]; if (Math.abs(difference) > maxDifference) maxDifference = Math.abs(difference); } return maxDifference; }
public void setAndUpdate(RigidBodyTransform transform) { transform.get(rotation, translation); setAndUpdate(rotation, translation); }
public void setLidarPose(RigidBodyTransform lidarTransformToWorld) { Point3d position = new Point3d(); Quat4d orientation = new Quat4d(); lidarTransformToWorld.get(orientation, position); lidarPosePacketToSend.set(new LidarPosePacket(position, orientation)); }
public static void setGeoregressionTransformFromVecmath(RigidBodyTransform vecmathTransform, Se3_F64 georegressionTransform) { double[] m1 = new double[16]; vecmathTransform.get(m1); double[][] rot = new double[][] { {m1[0], m1[1], m1[2]}, {m1[4], m1[5], m1[6]}, {m1[8], m1[9], m1[10]} }; DenseMatrix64F denseMatrix64F = new DenseMatrix64F(rot); georegressionTransform.setRotation(denseMatrix64F); georegressionTransform.setTranslation(m1[3], m1[7], m1[11]); }
/** * Computes the interpolation between the two transforms using the alpha parameter to control the blend. * Note that the transforms must have a proper rotation matrix, meaning it satisfies: R'R = I and det(R) = 1 * @param transform1 * @param transform2 * @param alpha Ranges from [0, 1], where return = (1- alpha) * tansform1 + (alpha) * transform2 * @return return = (1- alpha) * tansform1 + alpha * transform2 */ public void computeInterpolation(RigidBodyTransform transform1, RigidBodyTransform transform2, RigidBodyTransform result, double alpha) { alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); transform1.get(transform1Quaternion, transform1Translation); transform2.get(transform2Quaternion, transform2Translation); interpolatedTranslation.interpolate(transform1Translation, transform2Translation, alpha); interpolatedQuaternion.interpolate(transform1Quaternion, transform2Quaternion, alpha); result.setRotationAndZeroTranslation(interpolatedQuaternion); result.setTranslation(interpolatedTranslation); }
public void transform(RigidBodyTransform transform) { Matrix3d rotation = new Matrix3d(); Vector3d translation = new Vector3d(); transform.get(rotation, translation); translate(translation); rotate(rotation); }
public void transform(RigidBodyTransform transform) { Matrix3d rotation = new Matrix3d(); Vector3d translation = new Vector3d(); transform.get(rotation, translation); translate(translation); rotate(rotation); }
public void updateCamera() { if (cameraController != null) { cameraController.computeTransform(cameraTransform); cameraTransform.get(cameraRotation, cameraPosition); setLocationInZUpCoordinates(cameraPosition); setRotationInZUpcoordinates(cameraRotation); setHorizontalFoVInRadians((float) cameraController.getHorizontalFieldOfViewInRadians()); setClipDistanceNear((float) cameraController.getClipNear()); setClipDistanceFar((float) cameraController.getClipFar()); } updateFrustrum(); }
@Override public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3d planeNormal) { Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); double yaw = RotationTools.computeYaw(orientation); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); footstep.setSolePose(new FramePose(ReferenceFrame.getWorldFrame(), new Point3d(position), orientation)); }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap){ FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep); //set to the sole pose Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); originalFootstep.setLocation(new Point3d(position)); originalFootstep.setOrientation(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints() != null){ throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points"); } footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints()); footstep.setFootstepType(type); FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setSolePose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(originalFootstep.getTrajectoryType()); return type; }
private static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D) { javax.vecmath.Quat4f quat = new javax.vecmath.Quat4f(); javax.vecmath.Vector3f vector = new javax.vecmath.Vector3f(); transform3D.get(quat, vector); Vector3f jmeVector = new Vector3f(vector.getX(), vector.getY(), vector.getZ()); Quaternion jmeQuat = new Quaternion(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f)); return ret; }
@Override public void update(float tpf) { synchronized (lidarTransform) { lidarTransform.get(j3dOrientation, j3dPosition); lidarDistortionProcessor.setTransform(lidarTransform, time); } JMEDataTypeUtils.packVecMathTuple3dInJMEVector3f(j3dPosition, jmePosition); JMEDataTypeUtils.packVectMathQuat4dInJMEQuaternion(j3dOrientation, jmeOrientation); JMEGeometryUtils.transformFromZupToJMECoordinates(jmePosition); jmeOrientation.multLocal(scsToJMECameraRotation); JMEGeometryUtils.transformFromZupToJMECoordinates(jmeOrientation); for (int i = 0; i < numberOfCameras; i++) { lidarSceneViewPort[i].setLocation(jmePosition); if (cameraRotations[i] == null) { lidarSceneViewPort[i].setRotation(jmeOrientation); } else { jmeOrientation.mult(cameraRotations[i], jmeCameraOrientation); lidarSceneViewPort[i].setRotation(jmeCameraOrientation); } } }
public static Transform j3dTransform3DToJMETransform(RigidBodyTransform transform3D) { Quat4f quat = new Quat4f(); javax.vecmath.Vector3f vector = new javax.vecmath.Vector3f(); transform3D.get(quat, vector); Vector3f jmeVector = new Vector3f(vector.getX(), vector.getY(), vector.getZ()); Quaternion jmeQuat = new Quaternion(quat.getX(), quat.getY(), quat.getZ(), quat.getW()); Transform ret = new Transform(jmeVector, jmeQuat, new Vector3f(1.0f, 1.0f, 1.0f)); return ret; }
/** * Precomputes Ad^{T} * I since it is used twice in Adot*v and once in computing A. */ private void precomputeAdjointTimesInertia() { tempAdjoint.zero(); for(int i = 0; i<jointList.length; i++) { rigidBodies[i].getInertia().getExpressedInFrame().getTransformToDesiredFrame(tempTransform, centerOfMassFrame); tempTransform.get(tempMatrix3d,tempVector); set3By3MatrixBlock(tempAdjoint, 0, 0, tempMatrix3d); set3By3MatrixBlock(tempAdjoint, 3, 3, tempMatrix3d); MatrixTools.toTildeForm(tempPTilde, tempVector); tempPTilde.mul(tempMatrix3d); set3By3MatrixBlock(tempAdjoint, 0, 3, tempPTilde); rigidBodies[i].getInertia().getMatrix(tempInertiaMatrix); CommonOps.mult(tempAdjoint, tempInertiaMatrix, denseAdjTimesI[i]); } }
public void update(long timestamp) { fullRobot.updateFrames(); if(rootJoint != null) { RigidBodyTransform rootTransform = rootJoint.getJointTransform3D(); rootTransform.get(tempOrientation, tempPosition); robot.setOrientation(tempOrientation); robot.setPositionInWorld(tempPosition); } for (ImmutablePair<OneDegreeOfFreedomJoint, OneDoFJoint> jointPair : revoluteJoints) { OneDegreeOfFreedomJoint pinJoint = jointPair.getLeft(); OneDoFJoint revoluteJoint = jointPair.getRight(); pinJoint.setQ(revoluteJoint.getQ()); pinJoint.setQd(revoluteJoint.getQd()); pinJoint.setTau(revoluteJoint.getTau()); } robot.setTime(robot.getTime() + updateDT); if (scs != null) { scs.tickAndUpdate(); } }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap){ FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep); //set to the sole pose Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); originalFootstep.setLocation(new Point3d(position)); originalFootstep.setOrientation(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints()); footstep.setFootstepType(type); FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setSolePose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(originalFootstep.getTrajectoryType()); return type; }
private Transform getRosTransform(RigidBodyTransform transform3d) { Transform transform = topicMessageFactory.newFromType(Transform._TYPE); Quaternion rot = topicMessageFactory.newFromType(Quaternion._TYPE); Vector3 trans = topicMessageFactory.newFromType(Vector3._TYPE); Quat4d quat4d = new Quat4d(); Vector3d vector3d = new Vector3d(); transform3d.get(quat4d, vector3d); rot.setW(quat4d.getW()); rot.setX(quat4d.getX()); rot.setY(quat4d.getY()); rot.setZ(quat4d.getZ()); transform.setRotation(rot); trans.setX(vector3d.getX()); trans.setY(vector3d.getY()); trans.setZ(vector3d.getZ()); transform.setTranslation(trans); return transform; }
private Transform getRosTransform(RigidBodyTransform transform3d) { Transform transform = topicMessageFactory.newFromType(Transform._TYPE); Quaternion rot = topicMessageFactory.newFromType(Quaternion._TYPE); Vector3 trans = topicMessageFactory.newFromType(Vector3._TYPE); Quat4d quat4d = new Quat4d(); Vector3d vector3d = new Vector3d(); transform3d.get(quat4d, vector3d); rot.setW(quat4d.getW()); rot.setX(quat4d.getX()); rot.setY(quat4d.getY()); rot.setZ(quat4d.getZ()); transform.setRotation(rot); trans.setX(vector3d.getX()); trans.setY(vector3d.getY()); trans.setZ(vector3d.getZ()); transform.setTranslation(trans); return transform; }
public Footstep.FootstepType snapFootstep(Footstep footstep, List<Point3d> pointList, double defaultHeight) { FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep); //set to the sole pose Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); originalFootstep.setLocation(new Point3d(position)); originalFootstep.setOrientation(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, pointList, defaultHeight); footstep.setFootstepType(type); footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints()); FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setSolePose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(originalFootstep.getTrajectoryType()); return type; }