/** * @return copy of the angular part as a FrameVector */ public FrameVector getLinearPartAsFrameVectorCopy() { return new FrameVector(expressedInFrame, linearPart); }
public FrameVector getInitialHandPoseAngularVelocity(ReferenceFrame referenceFrame) { return new FrameVector(referenceFrame); } }
@Override public FrameVector getJointAxis() { return new FrameVector(jointAxis); }
public BiasProcessModelElement(ControlFlowOutputPort<FrameVector> statePort, ReferenceFrame frame, String name, YoVariableRegistry registry) { super(statePort, TimeDomain.CONTINUOUS, false, SIZE, name, registry); this.biasPort = statePort; this.bias = new FrameVector(frame); this.biasDelta = new FrameVector(frame); }
public SphericalJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame) { super(name, predecessor, beforeJointFrame); this.afterJointFrame = new FloatingInverseDynamicsJointReferenceFrame(name, beforeJointFrame); this.jointAngularVelocity = new FrameVector(afterJointFrame); this.jointAngularAcceleration = new FrameVector(afterJointFrame); this.jointAngularAccelerationDesired = new FrameVector(afterJointFrame); }
public AngularAccelerationProcessModelElement(String name, ReferenceFrame estimationFrame, YoVariableRegistry registry, ControlFlowOutputPort<FrameVector> angularAccelerationStatePort, ControlFlowInputPort<FrameVector> angularAccelerationInputPort) { super(angularAccelerationStatePort, TimeDomain.DISCRETE, false, SIZE, name, registry); this.angularAccelerationStatePort = angularAccelerationStatePort; this.angularAccelerationInputPort = angularAccelerationInputPort; this.angularAcceleration = new FrameVector(estimationFrame); this.angularAccelerationDelta = new FrameVector(estimationFrame); this.estimationFrame = estimationFrame; computeAngularAccelerationInputMatrixBlock(); }
public static FrameVector generateRandomFrameVector(Random random, ReferenceFrame frame) { FrameVector randomVector = new FrameVector(frame, RandomTools.generateRandomVector(random)); return randomVector; }
public static FrameVector computeForce(FramePoint centerOfMass, FramePoint cmp, double fZ) { FrameVector force = new FrameVector(centerOfMass); computeForce(force, centerOfMass, cmp, fZ); return force; }
public TranslationReferenceFrame(String frameName, ReferenceFrame parentFrame) { super(frameName, parentFrame, false, false, parentFrame.isZupFrame()); originVector = new FrameVector(parentFrame); }
public YoFrameVectorControlFlowOutputPort(ControlFlowElement controlFlowElement, String namePrefix, ReferenceFrame frame, YoVariableRegistry registry) { super(namePrefix, controlFlowElement); yoFrameVector = new YoFrameVector(namePrefix, frame, registry); super.setData(new FrameVector(frame)); }
public FrameVector getLinearVelocityCopy() { FrameVector linearVelocityCopy = new FrameVector(getReferenceFrame()); getLinearVelocity(linearVelocityCopy); return linearVelocityCopy; }
public FrameVector getInitialVelocity() { return new FrameVector(getReferenceFrame(), getV0Array()); } }
public FrameVector getAngularVelocityCopy() { FrameVector angularVelocityCopy = new FrameVector(getReferenceFrame()); getAngularVelocity(angularVelocityCopy); return angularVelocityCopy; }
public SimulatedLinearAccelerationSensor(String name, RigidBody rigidBody, ReferenceFrame measurementFrame, SpatialAccelerationCalculator spatialAccelerationCalculator, Vector3d gravitationalAcceleration, YoVariableRegistry registry) { this.rigidBody = rigidBody; this.measurementFrame = measurementFrame; this.spatialAccelerationCalculator = spatialAccelerationCalculator; this.gravitationalAcceleration = new FrameVector(ReferenceFrame.getWorldFrame(), gravitationalAcceleration); this.yoFrameVectorPerfect = new YoFrameVector(name + "Perfect", measurementFrame, registry); this.yoFrameVectorNoisy = new YoFrameVector(name + "Noisy", measurementFrame, registry); }
public FrameVector getTranslationNOTDueToRotationAboutFrame(FramePose otherPose) { checkReferenceFrameMatch(otherPose); RigidBodyTransform transformToOtherPose = getTransformFromThisToThat(otherPose); FrameVector ret = new FrameVector(referenceFrame); transformToOtherPose.getTranslation(ret.tuple); return ret; }
public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis, boolean isPartOfClosedKinematicLoop) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; return new PassiveRevoluteJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis), isPartOfClosedKinematicLoop); }
private static double computeMinX(ContactablePlaneBody contactablePlaneBody) { FrameVector back = new FrameVector(contactablePlaneBody.getSoleFrame(), -1.0, 0.0, 0.0); List<FramePoint> maxBack = DesiredFootstepCalculatorTools.computeMaximumPointsInDirection(contactablePlaneBody.getContactPointsCopy(), back, 1); return maxBack.get(0).getX(); }
public static Vector3d getTransformedVector(Vector3d vector3d, RigidBodyTransform transform3D) { ReferenceFrame ending = ReferenceFrame.constructARootFrame("ending", false, true, true); ReferenceFrame starting = ReferenceFrame.constructFrameWithUnchangingTransformToParent("starting", ending, transform3D, false, true, true); FrameVector framePoint = new FrameVector(starting, vector3d); framePoint.changeFrame(ending); return framePoint.getVector(); }
public PrismaticJoint(String name, RigidBody predecessor, ReferenceFrame beforeJointFrame, FrameVector jointAxis) { super(name, predecessor, beforeJointFrame, new PrismaticJointReferenceFrame(name, beforeJointFrame, jointAxis)); this.jointAxis = new FrameVector(jointAxis); this.unitJointTwist = new Twist(afterJointFrame, beforeJointFrame, afterJointFrame, jointAxis.getVector(), new Vector3d()); }
public ReferenceFrame copyAndAimAxisAtPoint(Axis axisToAlign, FramePoint targetToAimAt) { ReferenceFrame initialFrame = targetToAimAt.getReferenceFrame(); targetToAimAt.changeFrame(this); FrameVector targetRelativeToCurrentFrame = new FrameVector(this, targetToAimAt.getX(), targetToAimAt.getY(), targetToAimAt.getZ()); targetToAimAt.changeFrame(initialFrame); return copyAndAlignAxisWithVector(axisToAlign, targetRelativeToCurrentFrame); }