public void doPositionControl(FramePose desiredEndEffectorPose, Twist desiredEndEffectorTwist, SpatialAccelerationVector feedForwardEndEffectorSpatialAcceleration, RigidBody base) { twistCalculator.getRelativeTwist(currentTwist, base, endEffector); currentTwist.changeBodyFrameNoRelativeTwist(endEffectorFrame); currentTwist.changeFrame(endEffectorFrame); se3pdController.compute(acceleration, desiredEndEffectorPose, desiredEndEffectorTwist, feedForwardEndEffectorSpatialAcceleration, currentTwist); }