public double getDesiredElbowAngle(RobotSide robotSide) { return getArmJointAngles(robotSide)[3]; }
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); }
return symmetricArmPose(0.3, halfPi - 0.4, 0.05, 1.7, robotSide); case SMALL_CHICKEN_WINGS: return symmetricArmPose(0.3, halfPi - 0.6, 0.05, 1.7, robotSide); case LARGE_CHICKEN_WINGS: return symmetricArmPose(0.3, halfPi - 0.8, 0.05, 1.7, robotSide); case STRAIGHTEN_ELBOWS: return symmetricArmPose(0.3, halfPi - 0.6, 0.05, 1.0, robotSide); case SUPPINATE_ARMS_IN_A_LITTLE: return symmetricArmPose(0.3, halfPi - 0.6, 0.2, 1.7, robotSide); case ARMS_BACK: return symmetricArmPose(0.6, halfPi - 0.4, 0.05, 1.7, robotSide); case LARGER_CHICKEN_WINGS: return symmetricArmPose(0.3, halfPi - 1.0, 0.05, 1.7, robotSide); case ARMS_OUT_EXTENDED: return symmetricArmPose(0.3, halfPi - 1.0, 0.05, 0.9, robotSide); case FLYING: return symmetricArmPose(0.3, halfPi - 1.2, 0.05, 0.4, robotSide); case FLYING_PALMS_UP: return symmetricArmPose(0.3, halfPi - 1.2, -1.0, 0.4, robotSide); case FLEX_UP: return symmetricArmPose(0.0, 0.0, 0.0, 2.0, robotSide); case FLEX_DOWN: return symmetricArmPose(0.0, 0.0, 0.0, 1.4, robotSide); case FLYING_SUPPINATE_IN: return symmetricArmPose(0.3, halfPi - 1.2, 1.0, 0.4, robotSide); case FLYING_SUPPINATE_OUT: return symmetricArmPose(0.3, halfPi - 1.2, -1.0, 0.4, robotSide); case KARATE_KID_KRANE_KICK: return symmetricArmPose(0.0, 0.0, 0.0, 0.0, robotSide);
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); }
return symmetricArmPose(0.3, halfPi - 0.4, 0.05, 1.7, robotSide); case SMALL_CHICKEN_WINGS: return symmetricArmPose(0.3, halfPi - 0.6, 0.05, 1.7, robotSide); case LARGE_CHICKEN_WINGS: return symmetricArmPose(0.3, halfPi - 0.8, 0.05, 1.7, robotSide); case STRAIGHTEN_ELBOWS: return symmetricArmPose(0.3, halfPi - 0.6, 0.05, 1.0, robotSide); case SUPPINATE_ARMS_IN_A_LITTLE: return symmetricArmPose(0.3, halfPi - 0.6, 0.2, 1.7, robotSide); case ARMS_BACK: return symmetricArmPose(0.6, halfPi - 0.4, 0.05, 1.7, robotSide); case LARGER_CHICKEN_WINGS: return symmetricArmPose(0.3, halfPi - 1.0, 0.05, 1.7, robotSide); case ARMS_OUT_EXTENDED: return symmetricArmPose(0.3, halfPi - 1.0, 0.05, 0.9, robotSide); case FLYING: return symmetricArmPose(0.3, halfPi - 1.2, 0.05, 0.4, robotSide); case FLYING_PALMS_UP: return symmetricArmPose(0.3, halfPi - 1.2, -1.0, 0.4, robotSide); case FLEX_UP: return symmetricArmPose(0.0, 0.0, 0.0, 2.0, robotSide); case FLEX_DOWN: return symmetricArmPose(0.0, 0.0, 0.0, 1.4, robotSide); case FLYING_SUPPINATE_IN: return symmetricArmPose(0.3, halfPi - 1.2, 1.0, 0.4, robotSide); case FLYING_SUPPINATE_OUT: return symmetricArmPose(0.3, halfPi - 1.2, -1.0, 0.4, robotSide); case KARATE_KID_KRANE_KICK: return symmetricArmPose(0.0, 0.0, 0.0, 0.0, robotSide);
public double getDesiredElbowAngle(RobotSide robotSide) { return getArmJointAngles(robotSide)[3]; }
public SideDependentList<double[]> getArmJointAngles() { double[] leftArmJointAngles = armPose.getArmJointAngles(RobotSide.LEFT); double[] rightArmJointAngles = armPose.getArmJointAngles(RobotSide.RIGHT); return new SideDependentList<double[]>(leftArmJointAngles, rightArmJointAngles); }
public SideDependentList<double[]> getArmJointAngles() { double[] leftArmJointAngles = armPose.getArmJointAngles(RobotSide.LEFT); double[] rightArmJointAngles = armPose.getArmJointAngles(RobotSide.RIGHT); return new SideDependentList<double[]>(leftArmJointAngles, rightArmJointAngles); }