public void submitHumanoidArmPose(RobotSide robotSide, HumanoidArmPose armPose) { FrameOrientation desiredUpperArmOrientation = new FrameOrientation(fullRobotModel.getChest().getBodyFixedFrame(), armPose.getDesiredUpperArmYawPitchRoll()); double elbowAngle = robotSide.negateIfRightSide(armPose.getDesiredElbowAngle(robotSide)); double[] handOrientation = new double[3]; if (enableHandOrientation.getBooleanValue()) { handOrientation = armPose.getDesiredHandYawPitchRoll(); } FrameOrientation desiredHandOrientation = new FrameOrientation(lowerArmsFrames.get(robotSide), handOrientation); submitHandPose(robotSide, desiredUpperArmOrientation, elbowAngle, desiredHandOrientation, true); }
public void submitHumanoidArmPose(RobotSide robotSide, HumanoidArmPose armPose) { FrameQuaternion desiredUpperArmOrientation = new FrameQuaternion(); desiredUpperArmOrientation.setYawPitchRollIncludingFrame(fullRobotModel.getChest().getBodyFixedFrame(), armPose.getDesiredUpperArmYawPitchRoll()); double elbowAngle = robotSide.negateIfRightSide(armPose.getDesiredElbowAngle(robotSide)); double[] handOrientation = new double[3]; if (enableHandOrientation.getBooleanValue()) { handOrientation = armPose.getDesiredHandYawPitchRoll(); } FrameQuaternion desiredHandOrientation = new FrameQuaternion(); desiredHandOrientation.setYawPitchRollIncludingFrame(lowerArmsFrames.get(robotSide), handOrientation); submitHandPose(robotSide, desiredUpperArmOrientation, elbowAngle, desiredHandOrientation, true); }