private static Vector3D zAxis() { return new Vector3D(0.0, 0.0, 1.0); } }
private Vector3D generateRandomUpFacingNormal() { double normalX = random.nextDouble() * (2.0 * MAX_ABS_XY_NORMAL_VALUE) - MAX_ABS_XY_NORMAL_VALUE; double normalY = random.nextDouble() * (2.0 * MAX_ABS_XY_NORMAL_VALUE) - MAX_ABS_XY_NORMAL_VALUE; Vector3D normal = new Vector3D(normalX, normalY, 1.0); return normal; }
private void addBlockDown(int stepNumber, double stepDepth, double stepDownHeight) { Vector3D position = new Vector3D(0.5 * length + (stepNumber + 0.5) * stepDepth, 0.0, -blockHeight / 2.0 - (1 + stepNumber) * stepDownHeight); Vector3D dimensions = new Vector3D(stepDepth, blockWidth, blockHeight); addBlock(0.0, position, dimensions, blockColor); }
private void addRandomBlock(double forwardPosition, double height, double stepDepth) { Vector3D position = new Vector3D(0.5 * (length + stepDepth) + forwardPosition, 0.0, -blockHeight / 2.0 + height); Vector3D dimensions = new Vector3D(stepDepth, blockWidth, blockHeight); addBlock(0.0, position, dimensions, blockColor); }
public SquaredUpDRCDemo01Robot(double r, double theta, double psi) { super(new Vector3D(r*Math.cos(theta), r*Math.sin(theta), 0.0), theta+psi); } public SquaredUpDRCDemo01Robot(double z, double r, double theta, double psi)
public static Vector3D stringToVector3d(String vector) { String[] vecString = vector.split("\\s+"); Vector3D vector3d = new Vector3D(Double.parseDouble(vecString[0]), Double.parseDouble(vecString[1]), Double.parseDouble(vecString[2])); return vector3d; }
/** * Retrieves and returns a copy of the normal in world frame of this planar region. */ public Vector3D getNormal() { Vector3D normal = new Vector3D(); getNormal(normal); return normal; }
public Vector3D getAngularVelocityCopy() { Vector3D angularVelocityCopy = new Vector3D(); getAngularVelocity(angularVelocityCopy); return angularVelocityCopy; }
public Ray3d(Point3DReadOnly point, Vector3DReadOnly vector) { this.point = new Point3D(point); this.vector = new Vector3D(vector); }
/** * Update the camera translation after applying a translation offset in the world frame. * @param dx the translation offset along the world x-axis. * @param dy the translation offset along the world y-axis. * @param dz the translation offset along the world z-axis. */ public void updateWorldTranslation(double dx, double dy, double dz) { Vector3D shift = new Vector3D(dx, dy, dz); JavaFXTools.addEquals(translation, shift); }
public static SpatialAcceleration createGravitationalSpatialAcceleration(RigidBodyBasics rootBody, double gravity) { Vector3D gravitationalAcceleration = new Vector3D(0.0, 0.0, gravity); Vector3D zero = new Vector3D(); SpatialAcceleration rootAcceleration = new SpatialAcceleration(rootBody.getBodyFixedFrame(), ReferenceFrame.getWorldFrame(), rootBody.getBodyFixedFrame(), zero, gravitationalAcceleration); return rootAcceleration; }
public void useJointAxisGraphicsFromMassProperties() { getLinkGraphics().getGraphics3DInstructions().clear(); getLinkGraphics().combine(jointAxisGeometry, new Vector3D()); }
public void transform(RigidBodyTransform transform) { RotationMatrix rotation = new RotationMatrix(); Vector3D translation = new Vector3D(); transform.get(rotation, translation); translate(translation); rotate(rotation); }
private Axis createSDFJointAxis(OneDegreeOfFreedomJoint scsJoint) { Axis sdfJointAxis = new Axis(); Vector3D scsJointAxis = new Vector3D(); scsJoint.getJointAxis(scsJointAxis); String xyz = String.valueOf(scsJointAxis.getX()) + " " + String.valueOf(scsJointAxis.getY()) + " " + String.valueOf(scsJointAxis.getZ()); sdfJointAxis.setXyz(xyz); sdfJointAxis.setDynamics(createJointDynamics(scsJoint)); sdfJointAxis.setLimit(createJointLimit(scsJoint)); return sdfJointAxis; }
private static void setUpFloatingStair(CombinedTerrainObject3D combinedTerrainObject, double[] centerPoint, double width, double tread, double thickness, double stairTopHeight, double yawDegrees, AppearanceDefinition app) { double xCenter = centerPoint[0]; double yCenter = centerPoint[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3D(xCenter, yCenter, stairTopHeight - thickness / 2)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, tread, width, thickness), app); combinedTerrainObject.addTerrainObject(newBox); }
public static SO3TrajectoryPointMessage createSO3TrajectoryPointMessage(double time, QuaternionReadOnly orientation, Vector3DReadOnly angularVelocity) { SO3TrajectoryPointMessage message = new SO3TrajectoryPointMessage(); message.setTime(time); message.getOrientation().set(new Quaternion(orientation)); message.getAngularVelocity().set(new Vector3D(angularVelocity)); return message; }
private static RigidBodyTransform jmeTransformToTransform3D(Transform jmeTransform) { Quaternion jmeQuat = jmeTransform.getRotation(); Vector3f jmeVect = jmeTransform.getTranslation(); us.ihmc.euclid.tuple4D.Quaternion quat = new us.ihmc.euclid.tuple4D.Quaternion(jmeQuat.getX(), jmeQuat.getY(), jmeQuat.getZ(), jmeQuat.getW()); Vector3D vect = new Vector3D(jmeVect.getX(), jmeVect.getY(), jmeVect.getZ()); RigidBodyTransform ret = new RigidBodyTransform(quat, vect); return ret; }
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 static Vector3D mul3(Matrix3DReadOnly M, Vector3DReadOnly v) { Matrix3D V = new Matrix3D(); V.set(v.getX(), v.getX(), v.getX(), v.getY(), v.getY(), v.getY(), v.getZ(), v.getZ(), v.getZ()); Matrix3D MV = new Matrix3D(); MV.set(M); MV.multiply(V); return new Vector3D(MV.getM00(), MV.getM11(), MV.getM22()); }