public PrismaticJointReferenceFrame(String frameName, ReferenceFrame parentFrame, FrameVector axis) { super(frameName, parentFrame); axis.checkReferenceFrameMatch(parentFrame); this.axis = axis.getVectorCopy(); }
public RevoluteJointReferenceFrame(String frameName, ReferenceFrame parentFrame, FrameVector axis) { super(frameName, parentFrame); axis.checkReferenceFrameMatch(parentFrame); this.axis = axis.getVectorCopy(); }
private static OneDoFJoint cloneOneDoFJoint(OneDoFJoint original, String cloneSuffix, RigidBody clonePredecessor) { String jointNameOriginal = original.getName(); RigidBodyTransform jointTransform = original.getOffsetTransform3D(); Vector3d jointAxisCopy = original.getJointAxis().getVectorCopy(); OneDoFJoint clone; if (original instanceof RevoluteJoint) clone = ScrewTools.addRevoluteJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else if (original instanceof PrismaticJoint) clone = ScrewTools.addPrismaticJoint(jointNameOriginal + cloneSuffix, clonePredecessor, jointTransform, jointAxisCopy); else throw new RuntimeException("Unhandled type of " + OneDoFJoint.class.getSimpleName() + ": " + original.getClass().getSimpleName()); clone.setJointLimitLower(original.getJointLimitLower()); clone.setJointLimitUpper(original.getJointLimitUpper()); return clone; }
private void createJointAngleCorruptor(String namePrefix, String name, RevoluteJoint oneDoFJoint) { name = FormattingTools.addPrefixAndKeepCamelCase(namePrefix, name); final ReferenceFrame frameBeforeJoint = oneDoFJoint.getFrameBeforeJoint(); final Vector3d jointAxis = oneDoFJoint.getJointAxis().getVectorCopy(); final RigidBodyTransform preCorruptionTransform = new RigidBodyTransform(); final DoubleYoVariable offset = new DoubleYoVariable(name + "Offset", registry); VariableChangedListener jointOffsetChangedListener = new VariableChangedListener() { @Override public void variableChanged(YoVariable<?> v) { AxisAngle4d axisAngle = new AxisAngle4d(jointAxis, offset.getDoubleValue()); preCorruptionTransform.setRotationAndZeroTranslation(axisAngle); frameBeforeJoint.corruptTransformToParentPreMultiply(preCorruptionTransform); } }; offset.addVariableChangedListener(jointOffsetChangedListener); variableChangedListeners.add(jointOffsetChangedListener); }
/** * Creates a transform that transforms to the given point and rotates to make the z axis align with the * normal vector. */ private static RigidBodyTransform createTransformFromPointAndZAxis(FramePoint point, FrameVector zAxis) { RigidBodyTransform ret = new RigidBodyTransform(); ret.setRotation(GeometryTools.getAxisAngleFromZUpToVector(zAxis.getVectorCopy())); Vector3d translation = new Vector3d(); point.get(translation); ret.setTranslation(translation); return ret; }
private void computeAngularVelocityError() { orientationEstimator.getEstimatedAngularVelocity(estimatedAngularVelocityFrameVector); Vector3d estimatedAngularVelocity = estimatedAngularVelocityFrameVector.getVectorCopy(); Vector3d actualAngularVelocity = new Vector3d(); estimationJoint.physics.getAngularVelocityInBody(actualAngularVelocity); perfectAngularVelocity.set(actualAngularVelocity); actualAngularVelocity.sub(estimatedAngularVelocity); angularVelocityError.set(actualAngularVelocity.length()); }
private void computeCoMVelocityError() { Point3d comPoint = new Point3d(); Vector3d linearVelocity = new Vector3d(); Vector3d angularMomentum = new Vector3d(); double mass = robot.computeCOMMomentum(comPoint, linearVelocity, angularMomentum); linearVelocity.scale(1.0 / mass); perfectCoMVelocity.set(linearVelocity); orientationEstimator.getEstimatedCoMVelocity(estimatedCoMVelocityFrameVector); Vector3d estimatedCoMVelocity = estimatedCoMVelocityFrameVector.getVectorCopy(); estimatedCoMVelocity.sub(linearVelocity); comVelocityError.set(estimatedCoMVelocity.length()); }
public static ReferenceFrame constructReferenceFrameFromPointAndAxis(String frameName, FramePoint point, Axis axisToAlign, FrameVector alignAxisWithThis) { point.checkReferenceFrameMatch(alignAxisWithThis.getReferenceFrame()); FrameVector referenceNormal = new FrameVector(); switch (axisToAlign) { case X: referenceNormal.setIncludingFrame(point.getReferenceFrame(), 1.0, 0.0, 0.0); break; case Y: referenceNormal.setIncludingFrame(point.getReferenceFrame(), 0.0, 1.0, 0.0); break; case Z: referenceNormal.setIncludingFrame(point.getReferenceFrame(), 0.0, 0.0, 1.0); break; default: break; } AxisAngle4d rotationToDesired = new AxisAngle4d(); alignAxisWithThis.changeFrame(referenceNormal.getReferenceFrame()); GeometryTools.getAxisAngleFromFirstToSecondVector(referenceNormal.getVectorCopy(), alignAxisWithThis.getVectorCopy(), rotationToDesired); RigidBodyTransform transformToDesired = new RigidBodyTransform(); transformToDesired.setRotation(rotationToDesired); Vector3d translation = new Vector3d(); point.get(translation); transformToDesired.setTranslation(translation); return constructFrameWithUnchangingTransformToParent(frameName, point.getReferenceFrame(), transformToDesired); }