diff --git a/components/code/mtsIntuitiveResearchKitMTM.cpp b/components/code/mtsIntuitiveResearchKitMTM.cpp index 7db1ed78..c5d99eea 100644 --- a/components/code/mtsIntuitiveResearchKitMTM.cpp +++ b/components/code/mtsIntuitiveResearchKitMTM.cpp @@ -288,14 +288,18 @@ robManipulator::Errno mtsIntuitiveResearchKitMTM::InverseKinematics(vctDoubleVec } } - if (Manipulator->InverseKinematics(jointSet, cartesianGoal) == robManipulator::ESUCCESS) { - // find closest solution mod 2 pi - const double difference = m_kin_measured_js.Position()[JNT_WRIST_ROLL] - jointSet[JNT_WRIST_ROLL]; - const double differenceInTurns = nearbyint(difference / (2.0 * cmnPI)); - jointSet[JNT_WRIST_ROLL] = jointSet[JNT_WRIST_ROLL] + differenceInTurns * 2.0 * cmnPI; - return robManipulator::ESUCCESS; + robManipulator::Errno err = Manipulator->InverseKinematics(jointSet, cartesianGoal); + if (err != robManipulator::ESUCCESS) { + return err; } - return robManipulator::EFAILURE; + + // find closest position mod PI, and well within joint limits + const double current_wrist_roll = m_pid_measured_js.Position()[JNT_WRIST_ROLL]; + const double target_roll = jointSet[JNT_WRIST_ROLL]; + const double closest = closest_equivalent_roll(current_wrist_roll, target_roll); + //m_wrist_roll_virtual_offset = std::round((target_roll - closest)/cmnPI); + + return robManipulator::ESUCCESS; } void mtsIntuitiveResearchKitMTM::CreateManipulator(void) @@ -335,6 +339,28 @@ bool mtsIntuitiveResearchKitMTM::is_cartesian_ready(void) const return m_powered && m_encoders_biased; } +void mtsIntuitiveResearchKitMTM::UpdateStateJointKinematics(void) +{ + m_kin_measured_js = m_pid_measured_js; + m_kin_measured_js.Position()[JNT_WRIST_ROLL] += m_wrist_roll_virtual_offset*cmnPI; + m_kin_setpoint_js = m_pid_setpoint_js; + m_kin_setpoint_js.Position()[JNT_WRIST_ROLL] += m_wrist_roll_virtual_offset*cmnPI; +} + +void mtsIntuitiveResearchKitMTM::ToJointsPID(const vctDoubleVec & jointsKinematics, vctDoubleVec & jointsPID) +{ + jointsPID.Assign(jointsKinematics); + jointsPID[JNT_WRIST_ROLL] -= m_wrist_roll_virtual_offset*cmnPI; +} + +void mtsIntuitiveResearchKitMTM::servo_jp_internal(const vctDoubleVec & jp, const vctDoubleVec & jv) +{ + vctDoubleVec jointsPID; + jointsPID.SetSize(jp.size()); + ToJointsPID(jp, jointsPID); + mtsIntuitiveResearchKitArm::servo_jp_internal(jointsPID, jv); +} + void mtsIntuitiveResearchKitMTM::SetGoalHomingArm(void) { // if simulated, start at zero but insert tool so it can be used in cartesian mode @@ -580,11 +606,13 @@ void mtsIntuitiveResearchKitMTM::control_servo_cf_orientation_locked(void) CartesianPositionFrm.Rotation().From(mEffortOrientation); // important note, lock uses numerical IK as it finds a solution close to current position if (Manipulator->InverseKinematics(jointSet, CartesianPositionFrm) == robManipulator::ESUCCESS) { - // find closest solution mod 2 pi - const double difference = m_pid_measured_js.Position()[JNT_WRIST_ROLL] - jointSet[JNT_WRIST_ROLL]; - const double differenceInTurns = nearbyint(difference / (2.0 * cmnPI)); - jointSet[JNT_WRIST_ROLL] = jointSet[JNT_WRIST_ROLL] + differenceInTurns * 2.0 * cmnPI; - // initialize trajectory + // find closest solution mod PI, and well within joint limits + const double current_wrist_roll = m_pid_measured_js.Position()[JNT_WRIST_ROLL]; + const double target_roll = jointSet[JNT_WRIST_ROLL]; + const double closest = closest_equivalent_roll(current_wrist_roll, target_roll); + m_wrist_roll_virtual_offset = std::round((target_roll - closest)/cmnPI); + + // initialize trajectory m_trajectory_j.goal.Ref(number_of_joints_kinematics()).Assign(jointSet); m_trajectory_j.Reflexxes.Evaluate(m_servo_jp, m_servo_jv, @@ -676,8 +704,8 @@ void mtsIntuitiveResearchKitMTM::lock_orientation(const vctMatRot3 & orientation m_effort_orientation_locked = true; SetControlEffortActiveJoints(); // initialize trajectory - m_servo_jp.Assign(m_pid_measured_js.Position(), number_of_joints()); - m_servo_jv.Assign(m_pid_measured_js.Velocity(), number_of_joints()); + m_servo_jp.Assign(m_kin_measured_js.Position(), number_of_joints()); + m_servo_jv.Assign(m_kin_measured_js.Velocity(), number_of_joints()); m_trajectory_j.Reflexxes.Set(m_trajectory_j.v, m_trajectory_j.a, StateTable.PeriodStats.PeriodAvg(), @@ -712,3 +740,29 @@ void mtsIntuitiveResearchKitMTM::gravity_compensation(vctDoubleVec & efforts) efforts); } } + +double mtsIntuitiveResearchKitMTM::closest_equivalent_roll(double current_angle, double target_angle) const +{ + // find target angle, mod PI from current angle to minimize unnecessary rotations + const double delta = std::remainder(target_angle - current_angle, cmnPI); + double closest = current_angle + delta; + + // ensure we are within (min + PI, max - PI) range both to respect joint limits + // and to ensure we have +/-PI range of motion around us + const double roll_max = Manipulator->links[6].PositionMax() - cmnPI; + const double roll_min = Manipulator->links[6].PositionMin() + cmnPI; + + if (closest > roll_max) { + const double turns = (closest - roll_max)/cmnPI; + const double f_part = turns - std::trunc(turns); + const double offset = f_part > 1e-5 ? (1 - f_part)*cmnPI : 0.0; + closest = roll_max - offset; + } else if (closest < roll_min) { + const double turns = (roll_min - closest)/cmnPI; + const double f_part = turns - std::trunc(turns); + const double offset = f_part > 1e-5 ? (1 - f_part)*cmnPI : 0.0; + closest = roll_min + offset; + } + + return closest; +} diff --git a/components/code/robManipulatorMTM.cpp b/components/code/robManipulatorMTM.cpp index 45235236..3d4bd0e2 100644 --- a/components/code/robManipulatorMTM.cpp +++ b/components/code/robManipulatorMTM.cpp @@ -17,434 +17,246 @@ */ #include -#include -robManipulatorMTM::robManipulatorMTM(const std::vector linkParms, +#include +#include + +using namespace std::literals::string_literals; + +// coordinate system of MTM(R/L) link 0 is: +// z up, y away, x right +// when viewed from front of console + +// when all joints are at zero, the +// coordinate system of MTM(R/L) link 4 is: +// z left, y up, x towards +// when viewed from front of console. The +// rotation from link 0 frame to link 4 frame is: +// 0 0 -1 +// -1 0 0 +// 0 1 0 + +// when all joints are at zero, the +// coordinate system of MTM(R/L) link 7 is: +// z away, y right, x up +// when viewed from front of console. The +// rotation from link 4 frame to link 7 frame is: +// 0 0 -1 +// 1 0 0 +// 0 -1 0 + +// joints (directions specified relative to all-zero joint positions): +// 0: shoulder yaw. 0 is centered, positive is counterclockwise when viewed from above +// 1: shoulder pitch. 0 is straight down, positive is rotating forward +// 2: elbow pitch. 0 is straight forward, positive is rotating forward/up +// 3: gimbal platform yaw. 0 is outer side, depending on left/right MTM. positive is counterclockwise from above +// 4: wrist pitch. 0 is upright, positive is rotating forwards +// 5: wrist yaw. 0 is 90 from wrist, positive is counterclockwise from above +// 6: wrist roll. zero is horizontal, positive is *clockwise* from the front + +// inverse of frame 4->7 transformation given above +const vctRot3 robManipulatorMTM::rotation_78 = vctRot3(vctEulerYZXRotation3(cmnPI_2, 0.0, cmnPI_2)); + +robManipulatorMTM::robManipulatorMTM(const std::vector linkParms, const vctFrame4x4 &Rtw0) - : robManipulator(linkParms, Rtw0) -{ -} + : robManipulator(linkParms, Rtw0) {} robManipulatorMTM::robManipulatorMTM(const std::string &robotfilename, const vctFrame4x4 &Rtw0) - : robManipulator(robotfilename, Rtw0) -{ -} + : robManipulator(robotfilename, Rtw0) {} robManipulatorMTM::robManipulatorMTM(const vctFrame4x4 &Rtw0) - : robManipulator(Rtw0) + : robManipulator(Rtw0) {} + +#if CISST_HAS_JSON +robManipulator::Errno robManipulatorMTM::LoadRobot(const Json::Value & config) { + robManipulator::Errno result = robManipulator::LoadRobot(config); + if (result != robManipulator::ESUCCESS) { + return result; + } + + upper_arm_length = links[1].PStar().Norm(); + double forearm_length = links[2].PStar().Norm(); + double forearm_to_gimbal = links[3].PStar().Norm(); + + // pre-compute kinematic constants + elbow_to_gimbal_angle = std::atan(forearm_to_gimbal / forearm_length); + elbow_to_gimbal_length = std::sqrt(forearm_to_gimbal * forearm_to_gimbal + forearm_length * forearm_length); + + return robManipulator::ESUCCESS; } +#endif robManipulator::Errno -robManipulatorMTM::InverseKinematics(vctDynamicVector & q, - const vctFrame4x4 & Rts, +robManipulatorMTM::InverseKinematics(vctDynamicVector& q, + const vctFrame4x4& Rts, double CMN_UNUSED(tolerance), size_t CMN_UNUSED(Niterations), double CMN_UNUSED(LAMBDA)) { if (q.size() != links.size()) { - std::stringstream ss; - ss << "robManipulatorMTM::InverseKinematics: expected " << links.size() - << " joints values but received " << q.size(); - mLastError = ss.str(); + mLastError = "robManipulatorMTM::InverseKinematics: expected "s + std::to_string(links.size()) + + " joints values but received "s + std::to_string(q.size()); CMN_LOG_RUN_ERROR << mLastError << std::endl; return robManipulator::EFAILURE; } - if (links.size() == 0) { - mLastError = "robManipulatorMTM::InverseKinematics: the manipulator has no links"; + if (links.size() != 7) { + mLastError = "robManipulatorMTM::InverseKinematics: manipulator should have 7 links"s + + " but received "s + std::to_string(links.size()); CMN_LOG_RUN_ERROR << mLastError << std::endl; return robManipulator::EFAILURE; } - // if we encounter a joint limit, keep computing a solution but at - // the end return failure - bool hasReachedJointLimit = false; - - // take Rtw0 into account - vctFrm4x4 Rt07; - Rtw0.ApplyInverseTo(Rts, Rt07); - - q[0] = atan2l(Rt07.Translation().X(), - -Rt07.Translation().Y()); - - // arm is provided in ISI DH - const double l1 = 0.2794; - const double l1_sqr = l1 * l1; - - // create a triangle "above" forarm to find position - const double forarmBase = 0.3645; // from ISI documentation - const double forarmHeight = 0.1506; // for ISI documentation - const double l2_sqr = forarmBase * forarmBase + forarmHeight * forarmHeight; - const double l2 = sqrt(l2_sqr) ; - const double angleOffset = asinl(forarmHeight / l2); - - // project in plane formed by links 2 & 3 to find q2 and q3 (joint[1] and joint[2]) - const double x = -Rt07.Translation().Z(); - const double y = sqrt(Rt07.Translation().X() * Rt07.Translation().X() - + Rt07.Translation().Y() * Rt07.Translation().Y()); - - // 2 dof IK in plane - const double d_sqr = x * x + y * y; - const double d = sqrt(d_sqr); - const double a1 = atan2l(y, x); - const double a2 = acosl((l1_sqr - l2_sqr + d_sqr) / (2.0 * l1 * d)); - const double q1 = a1 - a2; - const double q2 = -acosl((l1_sqr + l2_sqr - d_sqr) / (2.0 * l1 * l2)); - - q[1] = q1; - q[2] = q2 - angleOffset + cmnPI_2; - - // check joint limits for first 3 joints - for (size_t joint = 0; joint < 3; joint++) { - if (ClampJointValueAndUpdateError(joint, q[joint], 1e-5)) { - hasReachedJointLimit = true; - } + // eliminate Rtw0 (link 0 to world transform) + vctFrm4x4 Rt07 = Rtw0.Inverse() * Rts; + + // shoulder and elbow angles entirely determine position, so we can solve + // for first three joints separately + vct3 position_ik = ShoulderElbowIK(Rt07.Translation()); + q[0] = position_ik[0]; + q[1] = position_ik[1]; + q[2] = position_ik[2]; + + // determine remaining transformation once shoulder/elbow is determined + q[3] = 0.0; + vctFrm4x4 Rt04 = ForwardKinematics(q, 4); + vctFrm4x4 Rt47 = Rt04.Inverse() * Rt07; + + // optimized yaw of platform to maximize range of motion + q[3] = ChoosePlatformYaw(vctRot3(Rt47.Rotation())); + + // once platform yaw has been chosen, need to re-compute link 4-7 frame transformation + Rt04 = ForwardKinematics(q, 4); + Rt47 = Rt04.Inverse() * Rt07; + + // once platform angle is chosen, wrist/gimbal IK is uniquely determined by + // the remaining portion of the desired overall transformation + vct3 gimbal_ik = WristGimbalIK(vctRot3(Rt47.Rotation())); + q[4] = gimbal_ik[0]; + q[5] = gimbal_ik[1]; + q[6] = gimbal_ik[2]; + + // copy prevents ODR-use, which in pre-C++17 requires a definition in addition to declaration + constexpr double tolerance = joint_limit_tolerance; + + // check+enforce all joint limits + bool joint_limit_reached = false; + for (size_t joint = 0; joint < 7; joint++) { + bool out_of_range = ClampJointValueAndUpdateError(joint, q[joint], tolerance); + joint_limit_reached = joint_limit_reached || out_of_range; } - // optimized placement of platform - // compute projection of roll axis on platform plane - q[3] = FindOptimalPlatformAngle(q, Rt07); - -// // compute orientation of platform -// const vctFrm4x4 Rt04 = this->ForwardKinematics(q, 4); -// vctFrm4x4 Rt47; -// Rt04.ApplyInverseTo(Rt07, Rt47); -// vctEulerZXZRotation3 closed57(Rt47.Rotation()); - -// // applying DH offsets -// q[4] = closed57.alpha() + cmnPI_2; -// q[5] = -closed57.beta() + cmnPI_2; -// q[6] = closed57.gamma() + cmnPI; - - // Or Use this function to calculate all the joints in the Gimbal - ComputeGimbalIK(q, Rt07); - - if (hasReachedJointLimit) { - return robManipulator::EFAILURE; - } - - return robManipulator::ESUCCESS; + // return failure if target pose is not achievable within joint limits + return joint_limit_reached ? EFAILURE : ESUCCESS; } +// solve for triangle's interior angle opposite from side c +double robManipulatorMTM::SolveTriangleInteriorAngle(double side_a, double side_b, double side_c) +{ + // law of cosines: c^2 = a^2 + b^2 - 2ab*cos(gamma) + // where triangle has sides a, b, c and gamma is the angle opposite side c + double numerator = side_a * side_a + side_b * side_b - side_c * side_c; + double denominator = 2 * side_a * side_b; + double cos_gamma = numerator / denominator; + return std::acos(cos_gamma); +} -int method = 2; -double q3_pre = 0.0; +vct3 robManipulatorMTM::ShoulderElbowIK(const vct3& position_07) const { + // shoulder yaw is just angle in transverse/horizontal plane + // coordinate system of shoulder is rotated 90 degrees, so we use X/-Y instead of Y/X + const double shoulder_yaw = std::atan2(position_07.X(), -position_07.Y()); + const double shoulder_to_wrist_distance = position_07.Norm(); + + // shoulder-elbow-wrist is a triangle with three known side lengths + // so the shoulder-elbow-wrist angle is fully determined + const double theta = SolveTriangleInteriorAngle(upper_arm_length, elbow_to_gimbal_length, shoulder_to_wrist_distance); + // interior angle formed by shoulder-elbow-forearm + const double elbow_interior_angle = theta + elbow_to_gimbal_angle; + // elbow joint pitch is supplementary/exterior angle, with 90 degree offset + const double elbow_pitch = (cmnPI - elbow_interior_angle) - cmnPI_2; + + // angle of ground-shoulder-wrist + const double alpha = std::acos(-position_07.Z() / shoulder_to_wrist_distance); + // angle of elbow-shoulder-wrist + const double beta = SolveTriangleInteriorAngle(upper_arm_length, shoulder_to_wrist_distance, elbow_to_gimbal_length); + // shoulder pitch is ground-shoulder-elbow angle + const double shoulder_pitch = alpha - beta; + + return vct3(shoulder_yaw, shoulder_pitch, elbow_pitch); +} -double robManipulatorMTM::ComputeGimbalIK(vctDynamicVector &q, - const vctFrame4x4 &Rt07) const +vct3 robManipulatorMTM::WristGimbalIK(const vctRot3& rotation_47) const { - vctEulerYZXRotation3 euler_offset; - // Rotation to align frame 7 with frame 4 - euler_offset.Assign(cmnPI_2, 0, -cmnPI_2); - - vctMatrixRotation3 Rt8; - vctEulerToMatrixRotation3(euler_offset, Rt8); - - vctFrm4x4 Rt08, Rt78; - Rt78.Rotation().Assign(Rt8); - Rt08 = Rt07 * Rt78; - - const vctFrm4x4 Rt04 = ForwardKinematics(q, 4); - - vctFrm4x4 Rt48; - Rt04.ApplyInverseTo(Rt08, Rt48); - - vctEulerZYXRotation3 closed48(Rt48.Rotation()); - - double q3, q4, q5, q6; - - - q4 = closed48.alpha(); - q5 = closed48.beta(); - q6 = -(closed48.gamma() - cmnPI); - - -// std::cerr << "ZYZ :" << q[4] << ", " << q[5] << ", " << q[6] << "\n" ; -// std::cerr << "ZYX :" << q4 << ", " << q5 << ", " << q6 << "\n" ; -// std::cerr << "-----------\n"; - - q[4] = q4; - q[5] = q5; - q[6] = q6; - - double scalar_mapping; - double range; - double centered_val; - double normalized_val; - - // Limits for Wrist Pitch (Joint 5 at Index 4) - // Consider the Wrist Pitch Joint at the Home position (0 Deg). - // The upwards pitch corresponds to a negative angle and and - // the downward pitch corresponds to a positive angle. - // The Upper Limit A and B are two points defined for the upward - // pitch angle and the Lower Limit A and B are two points defined - // for the lower pitch angle. Based on where the position of the joint - // angle is, a scalar mapping [-1.0 - 1.0] is calculated. - // The scalar mapping is a function of q4, i.e. scalar_mapping(q4) - // See the logic labeled Scalar Mapping Calculation Below - - // Changing the values of these two pairs of points, one - // can change the attenuation as well the center point of direction - // switching between each pair of points. - - // For example: consider lim_dn_a = 1.5 Rad and lim_dn_b = 2.0 Rad. The - // midpoint for these two limits is: - // mid_point_dn = 0.5 * (2.0 - 1.5) + 1.5 - // mid_point_dn => 1.75 Rad - // Thus the scalar mapping at these three points, i.e. [lim_dn_a, mid_point_dn, lim_dn_b] - // become: - - // scalar_mapping(lim_dn_a) = 1.0 - // scalar_mapping(mid_point_dn) = 0.0 - // scalar_mapping(lim_dn_b) = -1.0 - - // And for all values in between - // scalar_mapping(q4) = ((q4 - lim_dn_a) / (lim_dn_b - lim_dn_a) - 0.5) x 2.0 - - double lim_up_a = -2.0; // Upper Limit A for Pitch Joint - double lim_up_b = -1.5; // Upper Limit B for Pitch Joint - double lim_dn_a = 1.2; // Lower Limit A for Pitch Joint - double lim_dn_b = 1.8; // Lower Limit B for Pitch Joint - - // LOGIC: - // SCALAR MAPPING CALCULATION - if (lim_up_a < q[4] && q[4] < lim_up_b){ - range = lim_up_b - lim_up_a; - normalized_val = (q[4] - lim_up_a) / range; - centered_val = normalized_val - 0.5; - scalar_mapping = centered_val * 2; -// sign = 0; - } - else if (lim_up_b < q[4] && q[4] <= lim_dn_a){ - scalar_mapping = 1; - } - else if (lim_dn_a < q[4] && q[4] < lim_dn_b){ - range = lim_dn_b - lim_dn_a; - normalized_val = (q[4] - lim_dn_a) / range; - centered_val = normalized_val - 0.5; - scalar_mapping = -centered_val * 2; -// sign = 0; - } - else{ - scalar_mapping = -1; - } - -// std::cerr << "\r" << "Scalar Mapping: " << scalar_mapping; + // Add virtual frame 8 to align frame 7 with frame 4 + const vctRot3 rotation_48 = rotation_47 * rotation_78; - double Kp_3 = 2.0; - double Kd_3 = 0.1; - double e; + // decompose rotation from frame 4 to frame 8 into Euler angles, + vctEulerZYXRotation3 euler_rotation_decomposition(rotation_48); - e = q[5]; + // alignment of frame 4 means pitch/yaw/roll are Z/Y/X rotations + // roll joint axis is negative of frame's x axis + const double raw_wrist_pitch = euler_rotation_decomposition.alpha(); + const double raw_wrist_yaw = euler_rotation_decomposition.beta(); + const double raw_wrist_roll = -euler_rotation_decomposition.gamma(); - // Implicit dt incorporated into Kd_3 - q3 = Kp_3 * e * scalar_mapping + q[3] - Kd_3 * (q[3] - q3_pre); - q3_pre = q[3]; + // finds equivalent angle inside joint limits when possible, + // however does not actually clamp to joint limits + const double wrist_pitch = ClosestAngleToJointRange(raw_wrist_pitch, 2 * cmnPI, links[4].PositionMin(), links[4].PositionMax()); + const double wrist_yaw = ClosestAngleToJointRange(raw_wrist_yaw, 2 * cmnPI, links[5].PositionMin(), links[5].PositionMax()); + const double wrist_roll = ClosestAngleToJointRange(raw_wrist_roll, 2 * cmnPI, links[6].PositionMin(), links[6].PositionMax()); - // make sure we respect joint limits - const double q3Max = links[3].GetKinematics()->PositionMax(); - const double q3Min = links[3].GetKinematics()->PositionMin(); - if (q[3] > q3Max) { - q[3] = q3Max; - } else if (q[3] < q3Min) { - q[3] = q3Min; - } + return vct3(wrist_pitch, wrist_yaw, wrist_roll); } -// METHOD 0 -> RISHI'S METHOD -// METHOD 1 -> ADNAN'S METHOD -double robManipulatorMTM::FindOptimalPlatformAngle(const vctDynamicVector & q, - const vctFrame4x4 & Rt07) const +double robManipulatorMTM::ClosestAngleToJointRange( + double angle, double modulus, double min, double max +) { - // RISHI'S METHOD - if (method == 0) { - const vctFrm4x4 Rt03 = ForwardKinematics(q, 3); - vctFrm4x4 Rt37; - Rt03.ApplyInverseTo(Rt07, Rt37); - - // find the angle difference between the gripper and the third joint to calculate auto-correct angle - double angleDifference = acosl(-Rt37.Element(0, 2) / - sqrt(Rt37.Element(1, 2) * Rt37.Element(1, 2) + - Rt37.Element(0, 2) * Rt37.Element(0, 2))); - if (Rt37.Element(1, 2) > 0.0) { - angleDifference = -angleDifference; - } - - // calculate Angle Option 1 (The correct choice when right-side-up) - double option1 = angleDifference; - - // calculate Angle Option 2 (The correct choice when upside-down) - double option2 = option1 - cmnPI; - - // Normalize within joint space - if (option2 > cmnPI) { - option2 -= 2.0 * cmnPI; - } else if (option2 < (-3.0 * cmnPI_2)) { - option2 += 2.0 * cmnPI; - } - - // Normalize within joint space - if ((option2 < -cmnPI) - && (option2 > -3.0 * cmnPI_2) - && (q[3] > 0.0)) { - option2 += 2.0 * cmnPI; - } - - // Normalize within joint space - if ((option1 > cmnPI_2) - && (option1 < cmnPI) - && (q[3] < 0.0)) { - option1 -= 2.0 * cmnPI; - } - - // Choose either Option 1 or Option 2 based on which one is closer to the platform angle - double solution; - if (std::abs(q[3] - option2) < std::abs(q[3] - option1)) { - solution = option2; - } else { - solution = option1; - } - - // average with current position based on projection angle - const double cosProjectionAngle = std::abs(cos(q[4])); - double q3 = solution * cosProjectionAngle + q[3] * (1 - cosProjectionAngle); - - // make sure we respect joint limits - const double q3Max = links[3].GetKinematics()->PositionMax(); - const double q3Min = links[3].GetKinematics()->PositionMin(); - if (q3 > q3Max) { - q3 = q3Max; - } else if (q3 < q3Min) { - q3 = q3Min; - } - - std::cerr << "\r" << "Joint 3 Value: " << q3 << std::endl; - - return q3; - } + const double range_center = 0.5*(min + max); + return std::remainder(angle - range_center, modulus) + range_center; +} - // ADNAN'S METHOD - else if (method==1){ - - vctEulerYZXRotation3 euler_offset; - // Rotation to align frame 7 with frame 4 - euler_offset.Assign(cmnPI_2, 0, -cmnPI_2); - - vctMatrixRotation3 Rt8; - vctEulerToMatrixRotation3(euler_offset, Rt8); - - vctFrm4x4 Rt08, Rt78; - Rt78.Rotation().Assign(Rt8); - Rt08 = Rt07 * Rt78; - - const vctFrm4x4 Rt04 = ForwardKinematics(q, 4); - - vctFrm4x4 Rt48; - Rt04.ApplyInverseTo(Rt08, Rt48); - - vctEulerZYXRotation3 closed48(Rt48.Rotation()); - - // applying DH offsets - const double q4 = closed48.alpha(); - const double q5 = closed48.beta(); - - - vctDynamicVector qCopy(q); - -// ComputeGimbalIK(qCopy, Rt07); - - double scalar_mapping; - double range; - double centered_val; - double normalized_val; - - // Limits for Wrist Pitch (Joint 5 at Index 4) - // Consider the Wrist Pitch Joint at the Home position (0 Deg). - // The upwards pitch corresponds to a negative angle and and - // the downward pitch corresponds to a positive angle. - // The Upper Limit A and B are two points defined for the upward - // pitch angle and the Lower Limit A and B are two points defined - // for the lower pitch angle. Based on where the position of the joint - // angle is, a scalar mapping [-1.0 - 1.0] is calculated. - // The scalar mapping is a function of q4, i.e. scalar_mapping(q4) - // See the logic labeled Scalar Mapping Calculation Below - - // Changing the values of these two pairs of points, one - // can change the attenuation as well the center point of direction - // switching between each pair of points. - - // For example: consider lim_dn_a = 1.5 Rad and lim_dn_b = 2.0 Rad. The - // midpoint for these two limits is: - // mid_point_dn = 0.5 * (2.0 - 1.5) + 1.5 - // mid_point_dn => 1.75 Rad - // Thus the scalar mapping at these three points, i.e. [lim_dn_a, mid_point_dn, lim_dn_b] - // become: - - // scalar_mapping(lim_dn_a) = 1.0 - // scalar_mapping(mid_point_dn) = 0.0 - // scalar_mapping(lim_dn_b) = -1.0 - - // And for all values in between - // scalar_mapping(q4) = ((q4 - lim_dn_a) / (lim_dn_b - lim_dn_a) - 0.5) x 2.0 - - double lim_up_a = -2.0; // Upper Limit A for Pitch Joint - double lim_up_b = -1.5; // Upper Limit B for Pitch Joint - double lim_dn_a = 1.0; // Lower Limit A for Pitch Joint - double lim_dn_b = 2.0; // Lower Limit B for Pitch Joint - - // LOGIC: - // SCALAR MAPPING CALCULATION - if (lim_up_a < q4 && q4 < lim_up_b){ - range = lim_up_b - lim_up_a; - normalized_val = (q4 - lim_up_a) / range; - centered_val = normalized_val - 0.5; - scalar_mapping = centered_val * 2; -// scalar_mapping = 0; - } - else if (lim_up_b < q4 && q4 <= lim_dn_a){ - scalar_mapping = 1; - } - else if (lim_dn_a < q4 && q4 < lim_dn_b){ - range = lim_dn_b - lim_dn_a; - normalized_val = (q4 - lim_dn_a) / range; - centered_val = normalized_val - 0.5; - scalar_mapping = -centered_val * 2; -// scalar_mapping = 0; - } - else{ - scalar_mapping = -1; - } - - double Kp_3 = 1.0; - // double Kd_3 = 0.1; - double e; - double q3; - - e = q5; - - // Implicit dt incorporated into Kd_3 - double q3_increment = Kp_3 * q5 * scalar_mapping; - const double max_q3_dot = cmnPI * 0.1; // assume kHz - if (q3_increment > max_q3_dot) { - q3_increment = max_q3_dot; - } else if (q3_increment < -max_q3_dot) { - q3_increment = -max_q3_dot; - } - q3 = q[3] + q3_increment; -// q3 = Kp_3 * q5 * scalar_mapping + q[3]; // - Kd_3 * (q[3] - q3_pre); - q3_pre = q[3]; - - // make sure we respect joint limits - const double q3Max = links[3].GetKinematics()->PositionMax(); - const double q3Min = links[3].GetKinematics()->PositionMin(); - if (q3 > q3Max) { - q3 = q3Max; - } else if (q3 < q3Min) { - q3 = q3Min; - } - - return q3; +double robManipulatorMTM::ChoosePlatformYaw(const vctRot3& rotation_47) const +{ + // Add virtual frame 8 to align frame 7 with frame 4. + const vctRot3 rotation_48 = rotation_47 * rotation_78; + + // X-axis of frame 8 with respect to frame 4 gives orientation + // of gripper with respect to platform + auto x_axis = rotation_48 * vct3(1.0, 0.0, 0.0); + // phi is polar angle of the X-axis + const double sin_phi = x_axis.Y(); + // theta is azimuthal angle of the X-axis + const double theta = std::atan2(x_axis.X(), x_axis.Z()); + + // We want platform at 90-degree angle from projection of gripper into + // the horizontal plane, both to maximize range of motion and keep + // platform out of user's way. + const double raw_yaw = std::remainder(theta - cmnPI_2, 2 * cmnPI); + + // When gripper is pointed up, platform angle is effectively irrelevant, + // so as gripper gets closer to pointing straight up we move platform towards zero. + // When gripper points straight down, wrist pitch is very near lower joint limit so platform + // angle is very important (although this situation should be rare) + const double interpolation_factor = sin_phi < platform_alpha ? 1.0 : (1.0-sin_phi)/(1.0-platform_alpha); + const double interpolated_yaw = interpolation_factor * raw_yaw; + + const double max = links[3].PositionMax(); + const double min = links[3].PositionMin(); + + // Find yaw (mod 2PI) that is within joint limits, or find closest joint limit (mod 2PI). + const double yaw = ClosestAngleToJointRange(interpolated_yaw, 2 * cmnPI, min, max); + + // it is ok if yaw is outside joint limits, we can clamp and use + // other joints to make up for the difference + if (yaw < min) { + return min; + } else if (yaw > max) { + return max; + } else { + return yaw; } } + diff --git a/components/include/sawIntuitiveResearchKit/mtsIntuitiveResearchKitMTM.h b/components/include/sawIntuitiveResearchKit/mtsIntuitiveResearchKitMTM.h index b77a7404..61cbae2f 100644 --- a/components/include/sawIntuitiveResearchKit/mtsIntuitiveResearchKitMTM.h +++ b/components/include/sawIntuitiveResearchKit/mtsIntuitiveResearchKitMTM.h @@ -92,6 +92,10 @@ class CISST_EXPORT mtsIntuitiveResearchKitMTM: public mtsIntuitiveResearchKitArm bool is_joint_ready(void) const override; bool is_cartesian_ready(void) const override; + void UpdateStateJointKinematics(void) override; + void ToJointsPID(const vctDoubleVec & jointsKinematics, vctDoubleVec & jointsPID) override; + void servo_jp_internal(const vctDoubleVec & jp, const vctDoubleVec & jv) override; + // state related methods void SetGoalHomingArm(void) override; void TransitionEncodersBiased(void) override; @@ -118,6 +122,9 @@ class CISST_EXPORT mtsIntuitiveResearchKitMTM: public mtsIntuitiveResearchKitArm void gravity_compensation(vctDoubleVec & efforts) override; + double closest_equivalent_roll(double current_angle, double target_angle) const; + int m_wrist_roll_virtual_offset = 0; + // Functions for events struct { mtsFunctionWrite orientation_locked; diff --git a/components/include/sawIntuitiveResearchKit/robManipulatorMTM.h b/components/include/sawIntuitiveResearchKit/robManipulatorMTM.h index e0c2bd97..3c07dc7e 100644 --- a/components/include/sawIntuitiveResearchKit/robManipulatorMTM.h +++ b/components/include/sawIntuitiveResearchKit/robManipulatorMTM.h @@ -38,6 +38,11 @@ class robManipulatorMTM: public robManipulator ~robManipulatorMTM() {} + // NOTE: this is the *ONLY* supported way to load kinematic parameters for the MTMs +#if CISST_HAS_JSON + robManipulator::Errno LoadRobot(const Json::Value & config) override; +#endif + robManipulator::Errno InverseKinematics(vctDynamicVector & q, const vctFrame4x4 & Rts, @@ -45,10 +50,29 @@ class robManipulatorMTM: public robManipulator size_t Niterations = 1000, double LAMBDA = 0.001); - double FindOptimalPlatformAngle(const vctDynamicVector & q, - const vctFrame4x4 & Rt07) const; +private: + // length from shoulder to elbow + double upper_arm_length; + // angle formed by gimbal-elbow-forearm + double elbow_to_gimbal_angle; + // length from elbow to gimbal center of rotation + double elbow_to_gimbal_length; + + // rotation to align zero-position of frame 7 with frame 4 + static const vctRot3 rotation_78; + // constant to control platform angle behavior, see ChoosePlatformYaw() + static constexpr double platform_alpha = 0.8; + + // allowed (absolute) tolerance when enforcing joint limits + static constexpr double joint_limit_tolerance = 1e-5; + + static double SolveTriangleInteriorAngle(double side_a, double side_b, double side_c); + static double ClosestAngleToJointRange(double angle, double modulus, double min, double max); + + double ChoosePlatformYaw(const vctRot3& rotation_47) const; - double ComputeGimbalIK(vctDynamicVector & q, const vctFrame4x4 & Rt07) const; + vct3 ShoulderElbowIK(const vct3& position_07) const; + vct3 WristGimbalIK(const vctRot3& rotation_47) const; }; #endif // _robManipulatorMTM_h diff --git a/tests/README.md b/tests/README.md new file mode 100644 index 00000000..ebd99dfa --- /dev/null +++ b/tests/README.md @@ -0,0 +1,13 @@ +## Building and running tests + +Make sure catkin is configured to build CISST tests: +``` +catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCISST_BUILD_TESTS=ON +``` +Then clean and rebuild `saw_intuitive_research_kit_tests`. + +To run tests: +``` +~/catkin_ws/devel/bin/sawIntuitiveResearchKitTests --run +``` +*Remember to replace `devel` with appropriate branch if you are not working off of devel.* diff --git a/tests/robManipulatorTest.cpp b/tests/robManipulatorTest.cpp index 65cf2ea6..da772feb 100644 --- a/tests/robManipulatorTest.cpp +++ b/tests/robManipulatorTest.cpp @@ -80,8 +80,8 @@ class ManipulatorTestDataMTM: public ManipulatorTestData { jointErrorsAbsolute.AbsOf(jointErrors); std::string details = - "Actual joints: " + (SolutionJoints * cmn180_PI).ToString() + "\n" - "Solution : " + (ActualJoints * cmn180_PI).ToString() + "\n" + "Actual joints: " + (ActualJoints * cmn180_PI).ToString() + "\n" + "Solution : " + (SolutionJoints * cmn180_PI).ToString() + "\n" "Error : " + (jointErrors * cmn180_PI).ToString() + "\n"; // compare joint values