@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { frameOrientation.getTransform3D(transformToParent); } }
private void computeEstimationLinkToWorldTransform(RigidBodyTransform estimationLinkToWorldToPack, FrameOrientation estimationLinkOrientation) { // R_{estimation}^{w} estimationLinkOrientation.changeFrame(worldFrame); estimationLinkOrientation.getTransform3D(estimationLinkToWorldToPack); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); rotationPlane.changeFrame(parentFrame); rotationPlane.getTransform3D(transformToParent); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); rotationPlane.changeFrame(parentFrame); rotationPlane.getTransform3D(transformToParent); } };
public void getPose(RigidBodyTransform rigidBodyTransformToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); tempFrameOrientation.getTransform3D(rigidBodyTransformToPack); rigidBodyTransformToPack.setTranslation(tempFramePoint.getX(), tempFramePoint.getY(), tempFramePoint.getZ()); }
public void getPose(RigidBodyTransform rigidBodyTransformToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); tempFrameOrientation.getTransform3D(rigidBodyTransformToPack); rigidBodyTransformToPack.setTranslation(tempFramePoint.getX(), tempFramePoint.getY(), tempFramePoint.getZ()); }
private void computeEstimationLinkToWorldTransform(ReferenceFrame estimationFrame, RigidBodyTransform estimationLinkToWorldToPack, FramePoint centerOfMassWorld, FrameOrientation estimationLinkOrientation) { // r^{estimation} tempCenterOfMassBody.setToZero(estimationFrame); centerOfMassCalculator.getCenterOfMass(tempCenterOfMassBody); tempCenterOfMassBody.changeFrame(estimationFrame); // R_{estimation}^{w} estimationLinkOrientation.changeFrame(worldFrame); estimationLinkOrientation.getTransform3D(estimationLinkToWorldToPack); // R_{estimation}^{w} * r^{estimation} tempCenterOfMassBody.get(tempCenterOfMassBodyVector3d); estimationLinkToWorldToPack.transform(tempCenterOfMassBodyVector3d); // p_{estimation}^{w} = r^{w} - R_{estimation}^{w} r^{estimation} centerOfMassWorld.get(tempEstimationLinkPosition); tempEstimationLinkPosition.sub(tempCenterOfMassBodyVector3d); // H_{estimation}^{w} tempEstimationLinkPositionVector3d.set(tempEstimationLinkPosition); estimationLinkToWorldToPack.setTranslation(tempEstimationLinkPositionVector3d); }
private void computeEstimationLinkTransform(ReferenceFrame estimationFrame, RigidBodyTransform estimationLinkToWorldToPack, FramePoint centerOfMassWorld, FrameOrientation estimationLinkOrientation) { // r^{estimation} tempCenterOfMassBody.setToZero(estimationFrame); centerOfMassCalculator.getCenterOfMass(tempCenterOfMassBody); tempCenterOfMassBody.changeFrame(estimationFrame); // R_{estimation}^{w} estimationLinkOrientation.changeFrame(worldFrame); estimationLinkOrientation.getTransform3D(estimationLinkToWorldToPack); // R_{estimation}^{w} * r^{estimation} tempCenterOfMassBody.get(tempCenterOfMassBodyVector3d); estimationLinkToWorldToPack.transform(tempCenterOfMassBodyVector3d); // p_{estimation}^{w} = r^{w} - R_{estimation}^{w} r^{estimation} centerOfMassWorld.get(tempEstimationLinkPosition); tempEstimationLinkPosition.sub(tempCenterOfMassBodyVector3d); // H_{estimation}^{w} tempEstimationLinkPositionVector3d.set(tempEstimationLinkPosition); estimationLinkToWorldToPack.setTranslation(tempEstimationLinkPositionVector3d); }
temporaryDesiredHandOrientation.getTransform3D(desiredTransformForLowerArm); boolean success = inverseKinematicsForLowerArms.get(robotSide).solve(desiredTransformForLowerArm);
temporaryDesiredUpperArmOrientation.getTransform3D(desiredTransformForUpperArm); desiredTransformForUpperArm.multiply(desiredTransformForUpperArm, armZeroJointAngleConfigurationOffsets.get(robotSide)); boolean success = inverseKinematicsForUpperArms.get(robotSide).solve(desiredTransformForUpperArm);