us.ihmc.robotics.geometry.RigidBodyTransform.get()方法的使用及代码示例

本文整理了Java中us.ihmc.robotics.geometry.RigidBodyTransform.get方法的一些代码示例，展示了RigidBodyTransform.get的具体用法。这些代码示例主要来源于Github/Stackoverflow/Maven等平台，是从一些精选项目中提取出来的代码，具有较强的参考意义，能在一定程度帮忙到你。RigidBodyTransform.get方法的具体详情如下：
包路径：us.ihmc.robotics.geometry.RigidBodyTransform
类名称：RigidBodyTransform
方法名：get

RigidBodyTransform.get介绍

[英]Pack rotation part into Matrix3d and translation part into Tuple3d
[中]将旋转部分打包到Matrix3d中，将平移部分打包到Tuple3d中

代码示例

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

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;
}
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

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;
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

public void setAndUpdate(RigidBodyTransform transform)
{
transform.get(rotation, translation);
setAndUpdate(rotation, translation);
}

代码示例来源：origin: us.ihmc/IHMCPerception

public void setLidarPose(RigidBodyTransform lidarTransformToWorld)
{
Point3d position = new Point3d();
Quat4d orientation = new Quat4d();
lidarTransformToWorld.get(orientation, position);
lidarPosePacketToSend.set(new LidarPosePacket(position, orientation));
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

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]);
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

/**
* 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);
}

代码示例来源：origin: us.ihmc/IHMCGraphics3DDescription

public void transform(RigidBodyTransform transform)
{
Matrix3d rotation = new Matrix3d();
Vector3d translation = new Vector3d();
transform.get(rotation, translation);
translate(translation);
rotate(rotation);
}

代码示例来源：origin: us.ihmc/IHMCGraphicsDescription

public void transform(RigidBodyTransform transform)
{
Matrix3d rotation = new Matrix3d();
Vector3d translation = new Vector3d();
transform.get(rotation, translation);
translate(translation);
rotate(rotation);
}

代码示例来源：origin: us.ihmc/IHMCJMonkeyEngineToolkit

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();
}

代码示例来源：origin: us.ihmc/IHMCHumanoidRobotics

@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));
}

代码示例来源：origin: us.ihmc/IHMCHumanoidRobotics

@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;
}

代码示例来源：origin: us.ihmc/IHMCJMonkeyEngineToolkit

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;
}

代码示例来源：origin: us.ihmc/IHMCJMonkeyEngineToolkit

@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 {
lidarSceneViewPort[i].setLocation(jmePosition);
if (cameraRotations[i] == null)
{
lidarSceneViewPort[i].setRotation(jmeOrientation);
}
else
{
jmeOrientation.mult(cameraRotations[i], jmeCameraOrientation);
lidarSceneViewPort[i].setRotation(jmeCameraOrientation);
}
}
}

代码示例来源：origin: us.ihmc/IHMCJMonkeyEngineToolkit

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;
}

代码示例来源：origin: us.ihmc/IHMCRoboticsToolkit

/**
* 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 {
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]);
}
}

代码示例来源：origin: us.ihmc/IHMCSimulationToolkit

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 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();
}
}

代码示例来源：origin: us.ihmc/IHMCHumanoidRobotics

@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;
}

代码示例来源：origin: us.ihmc/IHMCROSTools

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;
}

代码示例来源：origin: us.ihmc/IHMCROSTools

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;
}

代码示例来源：origin: us.ihmc/IHMCHumanoidRobotics

public Footstep.FootstepType snapFootstep(Footstep footstep, List 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;
}