/** * Packs the position of this shape. * * @param tupleToPack the tuple in which the position of this shape is stored. Modified. */ public final void getPosition(Tuple3DBasics tupleToPack) { shapePose.getTranslation(tupleToPack); }
@Test public void testGetTranslation() throws Exception { Random random = new Random(2345L); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector3D translation = new Vector3D(); transform.getTranslation(translation); for (int row = 0; row < 3; row++) assertTrue(translation.getElement(row) == transform.getElement(row, 3)); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); translation.set(transform.getTranslationX(), transform.getTranslationY(), transform.getTranslationZ()); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); }
/** * Retrieves a point that lies in this planar region. This point is also used as the origin of * the local coordinate system of this planar region. * * @param pointToPack used to store the point coordinates. */ public void getPointInRegion(Point3DBasics pointToPack) { fromLocalToWorldTransform.getTranslation(pointToPack); }
public FrameVector3D getReferenceFrameTransInWorldFrame(ReferenceFrame frame) { Vector3D trans = new Vector3D(); frame.getTransformToDesiredFrame(worldFrame).getTranslation(trans); FrameVector3D ret = new FrameVector3D(worldFrame, trans); return ret; }
protected void setPositionInWorld(HumanoidFloatingRootJointRobot robot) { robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.getTranslation(offset); Vector3D positionInWorld = new Vector3D(); rootToWorld.getTranslation(positionInWorld); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = positionInWorld.getZ() - gc1.getPositionPoint().getZ(); offset.setZ(groundZ + pelvisToFoot); robot.setPositionInWorld(offset); }
public static void packEuclidRigidBodyTransformToGeometry_msgsPose(RigidBodyTransform pelvisTransform, Pose pose) { Vector3D point = new Vector3D(); pelvisTransform.getTranslation(point); us.ihmc.euclid.tuple4D.Quaternion rotation = new us.ihmc.euclid.tuple4D.Quaternion(); pelvisTransform.getRotation(rotation); packEuclidTuple3DAndQuaternionToGeometry_msgsPose(point, rotation, pose); }
public ArrayList<Double> getFootstepLengths(FootstepDataListMessage footStepList, FullHumanoidRobotModel fullRobotModel, WalkingControllerParameters walkingControllerParameters) { ArrayList<Double> footStepLengths = new ArrayList<Double>(); List<FootstepDataMessage> dataList = footStepList.getFootstepDataList(); for (int i = 0; i < dataList.size(); i++) { FootstepDataMessage step = dataList.get(i); footstepDataList.add(step); } FootstepDataMessage firstStepData = footstepDataList.remove(footstepDataList.size() - 1); RigidBodyTransform firstSingleSupportFootTransformToWorld = fullRobotModel.getFoot(RobotSide.fromByte(firstStepData.getRobotSide()).getOppositeSide()) .getBodyFixedFrame().getTransformToWorldFrame(); firstSingleSupportFootTransformToWorld.getTranslation(firstSingleSupportFootTranslationFromWorld); previousFootStepLocation.set(firstSingleSupportFootTranslationFromWorld); nextFootStepLocation.set(firstStepData.getLocation()); while (!footstepDataList.isEmpty()) { footStepLengths.add(previousFootStepLocation.distance(nextFootStepLocation)); previousFootStepLocation.set(nextFootStepLocation); nextFootStepLocation.set(footstepDataList.remove(footstepDataList.size() - 1).getLocation()); } double lastStepLength = previousFootStepLocation.distance(nextFootStepLocation); footStepLengths.add(lastStepLength); return footStepLengths; }
public void drawBox(BufferedImage image, RigidBodyTransform transform, double scale) { DenseMatrix64F rotation =new DenseMatrix64F(3,3); Vector3D translation = new Vector3D(); transform.getRotation(rotation); transform.getTranslation(translation); Se3_F64 targetToSensor = new Se3_F64(rotation,new Vector3D_F64(translation.getX(), translation.getY(), translation.getZ())); Graphics2D g2 = image.createGraphics(); g2.setStroke(new BasicStroke(4)); g2.setColor(java.awt.Color.RED); VisualizeFiducial.drawCube(targetToSensor, this.intrinsicParameters, scale, 2, g2); g2.finalize(); } }
private void assertLastTwoStepsCenterAroundEndPoint(String testDescription, ArrayList<Footstep> footsteps, Point2D endWorldPoint) { int numSteps = footsteps.size(); Footstep lastStep = footsteps.get(numSteps - 1); Footstep nextLastStep = footsteps.get(numSteps - 2); Vector3D lastStepPosition = new Vector3D(); Vector3D nextLastStepPosition = new Vector3D(); Vector3D positionInFrame = new Vector3D(); lastStep.getSoleReferenceFrame().getTransformToWorldFrame().getTranslation(lastStepPosition); nextLastStep.getSoleReferenceFrame().getTransformToWorldFrame().getTranslation(nextLastStepPosition); positionInFrame.interpolate(nextLastStepPosition, lastStepPosition, 0.5); Point2D positionInFrame2d = new Point2D(positionInFrame.getX(), positionInFrame.getY()); double endPositionOffset = endWorldPoint.distance(positionInFrame2d); assertEquals(testDescription + " footsteps must end near desired end", 0.0, endWorldPoint.distance(positionInFrame2d), endPositionTolerance); }
private void extractTandR(RigidBodyTransform tran, Vector3D T, Vector3D R) { tran.getTranslation(T); tran.getRotation(rotationMatrix); rotationMatrix.get(m); ConvertRotation3D_F64.matrixToEuler(m, EulerType.XYZ, euler); R.setX(euler[0]); R.setY(euler[1]); R.setZ(euler[2]); }
protected LineSegment3D getLineSegment(int index, float range) { Vector3D origin = new Vector3D(); RigidBodyTransform transform = new RigidBodyTransform(); getInterpolatedTransform(index, transform); transform.getTranslation(origin); return new LineSegment3D(new Point3D(origin), getPoint(index, range)); }
private DenseMatrix64F getSpatialErrorEndEffectorDesired() { transformEndEffectorToDesired.getTranslation(errorTranslationVector); transformEndEffectorToDesired.getRotation(errorQuat); errorAxisAngle.set(errorQuat); errorAngularVector.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorAngularVector.scale(errorAxisAngle.getAngle()); errorAngularVector.get(0, 0, spatialError); errorTranslationVector.get(3, 0, spatialError); return spatialError; }
protected Footstep createFootstepPlacedAtBipedfootLocation(RobotSide side) { ReferenceFrame soleFrame = soleFrames.get(side); Vector3D translation = new Vector3D(); Quaternion rotation = new Quaternion(); soleFrame.getTransformToWorldFrame().getTranslation(translation); soleFrame.getTransformToWorldFrame().getRotation(rotation); FramePose2D solePose2d = new FramePose2D(soleFrame, new Point2D(translation.getX(), translation.getY()), rotation.getYaw()); Footstep foot = createFootstep(side, solePose2d); return foot; }
private void setPelvisYoVariables(RigidBodyTransform pelvisTransform) { Vector3D pelvisTranslation = new Vector3D(); double[] yawPitchRoll = new double[3]; pelvisTransform.getTranslation(pelvisTranslation); Quaternion pelvisRotation = new Quaternion(); pelvisTransform.getRotation(pelvisRotation); YawPitchRollConversion.convertQuaternionToYawPitchRoll(pelvisRotation, yawPitchRoll); computedPelvisPositionX.set(pelvisTranslation.getX()); computedPelvisPositionY.set(pelvisTranslation.getY()); computedPelvisPositionZ.set(pelvisTranslation.getZ()); computedPelvisYaw.set(yawPitchRoll[0]); computedPelvisPitch.set(yawPitchRoll[1]); computedPelvisRoll.set(yawPitchRoll[2]); }
@Override protected void updateListeners(ArrayList<MocapRigidBody> lisftOfRigidbodies) { ArrayList<MocapRigidBody> convertedListOfMocapRigidBodies = new ArrayList<MocapRigidBody>(); for (MocapRigidBody mocapRigidBody : lisftOfRigidbodies) { mocapRbToMocapOrigin = new RigidBodyTransform(new Quaternion(mocapRigidBody.qx, mocapRigidBody.qy, mocapRigidBody.qz, mocapRigidBody.qw), new Vector3D(mocapRigidBody.xPosition, mocapRigidBody.yPosition, mocapRigidBody.zPosition)); mocapRbFrame.update(); mocapOriginFrame.update(); mocapRbZUpFrame.update(); FramePose3D pose = new FramePose3D(mocapRbZUpFrame, new Point3D(), new Quaternion()); pose.changeFrame(ReferenceFrame.getWorldFrame()); RigidBodyTransform r = new RigidBodyTransform(); pose.get(r); Vector3D position = new Vector3D(); r.getTranslation(position); Quaternion rotation = new Quaternion(); r.getRotation(rotation); convertedListOfMocapRigidBodies.add(new MocapRigidBody(mocapRigidBody.getId(), position, rotation, mocapRigidBody.getListOfAssociatedMarkers(), mocapRigidBody.dataValid)); } updateRigidBodiesListeners(convertedListOfMocapRigidBodies); }
private void computeDriftTransform() { RigidBodyTransform driftTransform = new RigidBodyTransform(); pelvisFrameFromMocap.update(); pelvisFrameFromRobotConfigurationDataPacket.update(); pelvisFrameFromMocap.getTransformToDesiredFrame(driftTransform, pelvisFrameFromRobotConfigurationDataPacket); Vector3D driftTranslation = new Vector3D(); driftTransform.getTranslation(driftTranslation); Quaternion driftRotation = new Quaternion(); driftTransform.getRotation(driftRotation); double[] driftRotationYPR = new double[3]; YawPitchRollConversion.convertQuaternionToYawPitchRoll(driftRotation, driftRotationYPR); mocapWorldToRobotWorldTransformX.set(driftTranslation.getX()); mocapWorldToRobotWorldTransformY.set(driftTranslation.getY()); mocapWorldToRobotWorldTransformZ.set(driftTranslation.getZ()); mocapWorldToRobotWorldTransformYaw.set(driftRotationYPR[0]); mocapWorldToRobotWorldTransformPitch.set(driftRotationYPR[1]); mocapWorldToRobotWorldTransformRoll.set(driftRotationYPR[2]); }
private void computeError(RigidBodyTransform desiredTransform) { jacobian.compute(); jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.setAndInvert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); errorAxisAngle.set(errorRotationMatrix); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorRotationVector.scale(0.2); errorTransform.getTranslation(errorTranslationVector); errorRotationVector.get(0, 0, spatialError); errorTranslationVector.get(3, 0, spatialError); }
private void computeError(RigidBodyTransform desiredTransform) { /* * B is base E is end effector D is desired end effector * * H^D_E = H^D_B * H^B_E = (H^B_D)^-1 * H^B_E * * convert rotation matrix part to rotation vector */ jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame()); errorTransform.setAndInvert(desiredTransform); errorTransform.multiply(actualTransform); errorTransform.getRotation(errorRotationMatrix); errorAxisAngle.set(errorRotationMatrix); axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ()); errorRotationVector.set(axis); errorRotationVector.scale(errorAxisAngle.getAngle()); errorTransform.getTranslation(errorTranslationVector); errorRotationVector.get(0, error); errorTranslationVector.get(3, error); errorScalar = NormOps.normP2(error); assert (exponentialCoordinatesOK()); }
public void receivedClockMessage(long totalNsecs) { RigidBodyTransform pelvisPoseInMocapFrame = atomicPelvisPose.get(); IMUDefinition[] imuDefinitions = fullRobotModel.getIMUDefinitions(); RobotConfigurationData robotConfigurationData = RobotConfigurationDataFactory.create(fullRobotModel.getOneDoFJoints(), forceSensorDefinitions, imuDefinitions); for(int sensorNumber = 0; sensorNumber < imuDefinitions.length; sensorNumber++) { IMUPacket imuPacket = robotConfigurationData.getImuSensorData().add(); imuPacket.getLinearAcceleration().set(RandomGeometry.nextVector3D32(random)); imuPacket.getOrientation().set(RandomGeometry.nextQuaternion32(random)); imuPacket.getAngularVelocity().set(RandomGeometry.nextVector3D32(random)); } robotConfigurationData.setRobotMotionStatus(RobotMotionStatus.STANDING.toByte()); robotConfigurationData.setTimestamp(totalNsecs); if(pelvisPoseInMocapFrame != null) { Vector3D translation = new Vector3D(); Quaternion orientation = new Quaternion(); pelvisPoseInMocapFrame.getTranslation(translation); pelvisPoseInMocapFrame.getRotation(orientation); robotConfigurationData.getRootTranslation().set(translation); robotConfigurationData.getRootOrientation().set(orientation); } fullRobotModel.updateFrames(); packetCommunicator.send(robotConfigurationData); }
public static void rigidBodyTransformToOpenCVTR(RigidBodyTransform transform, Mat tvec, Mat rvec) { Point3D translation = new Point3D(); transform.getTranslation(translation); AxisAngle axisAngle = new AxisAngle(); transform.getRotation(axisAngle); Vector3D rotVector = new Vector3D(axisAngle.getX(), axisAngle.getY(), axisAngle.getZ()); rotVector.normalize(); rotVector.scale(axisAngle.getAngle()); tvec.put(0, 0, translation.getX()); tvec.put(1, 0, translation.getY()); tvec.put(2, 0, translation.getZ()); rvec.put(0, 0, rotVector.getX()); rvec.put(1, 0, rotVector.getY()); rvec.put(2, 0, rotVector.getZ()); }