public static RigidBodyTransform yawPitchDegreesTransform(Vector3D center, double yawCCWDegrees, double pitchDownDegrees) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawPitchRoll(Math.toRadians(yawCCWDegrees), Math.toRadians(pitchDownDegrees), 0.0); location.setTranslation(center); return location; }
public static RigidBodyTransform transformFromJMECoordinatesToZup(Transform transform) { RigidBodyTransform modifiedTransform = new RigidBodyTransform(yUpToZupTransform); RigidBodyTransform temp = jmeTransformToTransform3D(transform); modifiedTransform.multiply(temp); return modifiedTransform; }
public static Transform fromPose3DToJMETransform(Pose3D pose3D) { RigidBodyTransform transform = new RigidBodyTransform(); pose3D.get(transform); return j3dTransform3DToJMETransform(transform); }
@Override public RigidBodyTransform getOffsetTransform3D() { return new RigidBodyTransform(jointToWrap.getFrameBeforeJoint().getTransformToParent()); }
/** * Creates a new spherical joint. * * @param name the name for the new joint. * @param predecessor the rigid-body connected to and preceding this joint. * @param jointOffset the offset in translation with respect to the frame after the parent joint. * Not modified. */ public SphericalJoint(String name, RigidBodyBasics predecessor, Tuple3DReadOnly jointOffset) { this(name, predecessor, new RigidBodyTransform(new Quaternion(), jointOffset)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testVerticalTranslation() { FootstepNode nodeToSnap = new FootstepNode(2.5, -0.5); RigidBodyTransform nodeTransform = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(nodeToSnap, nodeTransform); RigidBodyTransform transformToWorld = new RigidBodyTransform(); transformToWorld.setTranslationZ(-1.0); doAFullFootholdTest(transformToWorld, nodeToSnap); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform pinJointTransform = new RigidBodyTransform(); RigidBodyTransform newPose = new RigidBodyTransform(); pinJointTransform.setRotationYawAndZeroTranslation(steeringWheelPinJoint.getQYoVariable().getDoubleValue()); newPose.set(originalSteeringWheelPose); newPose.multiply(pinJointTransform); steeringWheelFrame.setPoseAndUpdate(newPose); super.updateAllGroundContactPointVelocities(); }
private FramePose3D getRobotFootPose(HumanoidFloatingRootJointRobot robot, RobotSide robotSide) { List<GroundContactPoint> gcPoints = robot.getFootGroundContactPoints(robotSide); Joint ankleJoint = gcPoints.get(0).getParentJoint(); RigidBodyTransform ankleTransformToWorld = new RigidBodyTransform(); ankleJoint.getTransformToWorld(ankleTransformToWorld); FramePose3D ret = new FramePose3D(); ret.set(ankleTransformToWorld); return ret; }
private static void setUpSlopedBox(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, double zCenter, double xLength, double yLength, double zLength, double slopeRadians, double yawDegrees, AppearanceDefinition app) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-slopeRadians); location.multiply(tilt); location.setTranslation(new Vector3D(xCenter, yCenter, zCenter)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, xLength, yLength, zLength), app); combinedTerrainObject.addTerrainObject(newBox); }
private FramePose2D getRobotFootPose2d(HumanoidFloatingRootJointRobot robot, RobotSide robotSide) { List<GroundContactPoint> gcPoints = robot.getFootGroundContactPoints(robotSide); Joint ankleJoint = gcPoints.get(0).getParentJoint(); RigidBodyTransform ankleTransformToWorld = new RigidBodyTransform(); ankleJoint.getTransformToWorld(ankleTransformToWorld); FramePose2D ret = new FramePose2D(); ret.setIncludingFrame(ReferenceFrame.getWorldFrame(), ankleTransformToWorld, false); return ret; }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform sliderJointTransform = new RigidBodyTransform(); RigidBodyTransform newButtonPose = new RigidBodyTransform(); buttonPushVector.scale(buttonSliderJoint.getQYoVariable().getDoubleValue()); sliderJointTransform.setTranslationAndIdentityRotation(buttonPushVector); buttonPushVector.normalize(); newButtonPose.set(originalButtonTransform); newButtonPose.multiply(sliderJointTransform); buttonFrame.setPoseAndUpdate(newButtonPose); super.updateAllGroundContactPointVelocities(); }
public void initialize(ReferenceFrame pelvisFrame, MocapRigidBody markerRigidBody) { RigidBodyTransform marker2ToMocapTransform = new RigidBodyTransform(markerRigidBody.getOrientation(), markerRigidBody.getPosition()); RigidBodyTransform worldToPelvisTransform = pelvisFrame.getTransformToWorldFrame(); worldToPelvisTransform.invert(); RigidBodyTransform worldToMocapTransform = new RigidBodyTransform(); worldToMocapTransform.multiply(marker2ToMocapTransform); worldToMocapTransform.multiply(pelvisToMarker2Transform); worldToMocapTransform.multiply(worldToPelvisTransform); mocapFrame = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("mocapFrame", ReferenceFrame.getWorldFrame(), worldToMocapTransform); initialized = true; }
@Override public void adjustFootstepWithoutHeightmap(FootstepDataMessage footstep, double height, Vector3D planeNormal) { Point3D position = footstep.getLocation(); Quaternion orientation = footstep.getOrientation(); RigidBodyTransform solePose = new RigidBodyTransform(); double yaw = orientation.getYaw(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); }
public static RigidBodyTransform shiftInSoleFrame(Vector2D shiftVector, RigidBodyTransform soleTransform) { RigidBodyTransform shiftTransform = new RigidBodyTransform(); shiftTransform.setTranslation(new Vector3D(shiftVector.getX(), shiftVector.getY(), 0.0)); soleTransform.multiply(shiftTransform); return soleTransform; }
public WandererWalkingControllerParameters(DRCRobotJointMap jointMap, boolean runningOnRealRobot) { this.jointMap = jointMap; this.runningOnRealRobot = runningOnRealRobot; this.toeOffParameters = new WandererToeOffParameters(); this.swingTrajectoryParameters = new WandererSwingTrajectoryParameters(runningOnRealRobot); this.steppingParameters = new WandererSteppingParameters(runningOnRealRobot); for (RobotSide robotSide : RobotSide.values()) { handPosesWithRespectToChestFrame.put(robotSide, new RigidBodyTransform()); } }
private double getAngleToPelvis(Point3D point, Point3D lidarOrigin) { RigidBodyTransform tf = new RigidBodyTransform(); ReferenceFrame.getWorldFrame().getTransformToDesiredFrame(tf, fullRobotModel.getPelvis().getBodyFixedFrame()); Point3D tfPoint = new Point3D(point); tf.transform(tfPoint); return Math.atan2(tfPoint.getY(), tfPoint.getX()); }
private TerrainObject3D setUpFlatGrid(int row, int column) { AppearanceDefinition gridAppearance = new YoAppearanceTexture("Textures/asphalt.png"); RigidBodyTransform location = new RigidBodyTransform(); location.appendTranslation(getWorldCoordinate(row, column)); location.appendTranslation(0.0, 0.0, -flatGridHeight / 2); return new RotatableBoxTerrainObject(new Box3D(location, gridLength, gridWidth, flatGridHeight), gridAppearance); }
private void addPlanarRegionAtZeroHeight(double xLocation, double yLocation) { ConvexPolygon2D polygon = new ConvexPolygon2D(); polygon.addVertex(0.3, 0.3); polygon.addVertex(-0.3, 0.3); polygon.addVertex(0.3, -0.3); polygon.addVertex(-0.3, -0.25); polygon.update(); PlanarRegion planarRegion = new PlanarRegion(new RigidBodyTransform(new AxisAngle(), new Vector3D(xLocation, yLocation, 0.0)), polygon); planarRegionsList.addPlanarRegion(planarRegion); }
private void addBlock(double yaw, Vector3D position, Vector3D dimensions, AppearanceDefinition color) { AxisAngle orientation = new AxisAngle(new Vector3D(0.0, 0.0, 1.0), yaw); RigidBodyTransform blockPose = new RigidBodyTransform(orientation, position); Box3D block = new Box3D(blockPose, dimensions.getX(), dimensions.getY(), dimensions.getZ()); terrainObject.addTerrainObject(new RotatableBoxTerrainObject(block, color)); }
private static PoseReferenceFrame createPlaneFrame(String name, Point3D planeReferencePoint, Vector3D planeSurfaceNormal) { PoseReferenceFrame planeFrame = new PoseReferenceFrame(name, ReferenceFrame.getWorldFrame()); RotationMatrix rotationFromNormal = computeRotationFromNormal(planeSurfaceNormal); RigidBodyTransform transform3D = new RigidBodyTransform(rotationFromNormal, new Vector3D(planeReferencePoint)); FramePose3D framePose = new FramePose3D(ReferenceFrame.getWorldFrame(), transform3D); planeFrame.setPoseAndUpdate(framePose); planeFrame.update(); return planeFrame; }