/** * Get the axis along which the variance is the least. * * @param thirdVectorToPack */ public void getThirdVector(Vector3D thirdVectorToPack) { thirdVectorToPack.set(thirdAxis); }
@Override protected void copy(Vector3D src, Vector3D dest) { dest.set(src); }
public void getRawIMULinearAcceleration(String imuName, Vector3D linearAccelerationToPack) { linearAccelerationToPack.set(rawIMULinearAccelerationMap.get(imuName)); } }
public void get(Vector3D translation) { translation.set(yoFramePose.getPosition()); } }
@Override public void set(EuclideanWaypoint other) { position.set(other.position); linearVelocity.set(other.linearVelocity); }
public void setBox(double lx, double ly, double lz, double centerX, double centerY, double centerZ) { boxSize.set(lx, ly, lz); boxCenter.set(centerX, centerY, centerZ); }
/** * Copies the {@code other} ellipsoid data into {@code this}. * * @param other the other ellipsoid to copy. Not modified. */ @Override public void set(Ellipsoid3D other) { setPose(other); radii.set(other.radii); }
private void computeDrift(double time, double alphaDecay, YoFrameVector3D angularVelocity, Quaternion driftToPack) { tempAngularVelocityForDrift.set(angularVelocity); tempAngularVelocity.scale(alphaDecay); RotationTools.integrateAngularVelocity(tempAngularVelocityForDrift, time, driftToPack); }
public static PlanOffsetStatus createPlanOffsetStatus(Vector3DReadOnly offsetVector) { PlanOffsetStatus message = new PlanOffsetStatus(); message.getOffsetVector().set(offsetVector); return message; }
public void surfaceNormalAt(double x, double y, double z, Vector3D normalToPack) { GroundPoint groundPoint = getGroundPoint(x, y, z); normalToPack.set(groundPoint.getNormal()); }
public static WrenchTrajectoryPointMessage createWrenchTrajectoryPointMessage(double time, Vector3DReadOnly torque, Vector3DReadOnly force) { WrenchTrajectoryPointMessage message = new WrenchTrajectoryPointMessage(); message.setTime(time); if (torque != null) message.getWrench().getTorque().set(torque); if (force != null) message.getWrench().getForce().set(force); return message; }
public void set(Ray3d ray3d) { this.getPoint().set(ray3d.getPoint()); this.getVector().set(ray3d.getVector()); }
public static KinematicsToolboxOutputStatus nextKinematicsToolboxOutputStatus(Random random) { KinematicsToolboxOutputStatus next = new KinematicsToolboxOutputStatus(); next.setJointNameHash(random.nextInt()); next.getDesiredJointAngles().add(RandomNumbers.nextFloatArray(random, random.nextInt(100), 1.0f)); next.getDesiredRootTranslation().set(EuclidCoreRandomTools.nextVector3D32(random)); next.getDesiredRootOrientation().set(EuclidCoreRandomTools.nextQuaternion32(random)); next.setSolutionQuality(random.nextDouble()); return next; }
private void computeInitialConstraintError(FramePoint3DReadOnly initialPosition, double initialTime) { trajectory.compute(initialTime); trajectoryFrame.checkReferenceFrameMatch(initialPosition.getReferenceFrame()); trajectory.getPosition(tempPosition); tempPosition.changeFrame(trajectoryFrame); initialConstraintPositionError.set(initialPosition); initialConstraintPositionError.sub(tempPosition); }
private void computeFinalConstraintError(FramePoint3DReadOnly finalPosition, FrameVector3DReadOnly finalVelocity, double finalTime) { computeFinalConstraintError(finalPosition, finalTime); trajectoryFrame.checkReferenceFrameMatch(finalVelocity.getReferenceFrame()); trajectory.getVelocity(tempVelocity); tempVelocity.changeFrame(trajectoryFrame); finalConstraintVelocityError.set(finalVelocity); finalConstraintVelocityError.sub(tempVelocity); }
public static SE3TrajectoryPointMessage nextSE3TrajectoryPointMessage(Random random) { SE3TrajectoryPointMessage next = new SE3TrajectoryPointMessage(); next.setTime(RandomNumbers.nextDoubleWithEdgeCases(random, 0.01)); next.getPosition().set(RandomGeometry.nextPoint3D(random, 1.0, 1.0, 1.0)); next.getOrientation().set(RandomGeometry.nextQuaternion(random)); next.getLinearVelocity().set(RandomGeometry.nextVector3D(random)); next.getAngularVelocity().set(RandomGeometry.nextVector3D(random)); return next; }
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; }
public static SO3TrajectoryPointMessage nextSO3TrajectoryPointMessage(Random random) { SO3TrajectoryPointMessage next = new SO3TrajectoryPointMessage(); next.setTime(RandomNumbers.nextDoubleWithEdgeCases(random, 0.01)); next.getOrientation().set(RandomGeometry.nextQuaternion(random)); next.getAngularVelocity().set(RandomGeometry.nextVector3D(random)); return next; }
public void recordCurrentState() { FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); rootJointTranslation.set(rootJoint.getJointPose().getPosition()); rootJointRotation.set(rootJoint.getJointPose().getOrientation()); yoRootJointTranslation.set(rootJointTranslation); yoRootJointRotation.set(rootJointRotation); }
private void addCylinderLinkGraphics(Vector3D linkStartPointOffsetFromParentJoint, double linkThickness, Vector3D linkVectorInWorld, AppearanceDefinition color, double trimBothEndsByThisMuch) { linkVectorCopy.set(linkVectorInWorld); Graphics3DObject linkGraphics = getLinkGraphics(); linkGraphics.identity(); linkGraphics.translate(linkStartPointOffsetFromParentJoint); linkVectorCopy.sub(linkStartPointOffsetFromParentJoint); linkGraphics.rotate(EuclidGeometryTools.axisAngleFromZUpToVector3D(linkVectorCopy)); linkGraphics.translate(0.0, 0.0, trimBothEndsByThisMuch); linkGraphics.addCylinder(linkVectorCopy.length() - trimBothEndsByThisMuch, linkThickness, color); }