From 7247d06e43ea4f1f1a42db8dca635e23b12f6160 Mon Sep 17 00:00:00 2001 From: Felix Maisonneuve Date: Wed, 6 Oct 2021 14:46:54 -0400 Subject: [PATCH 1/2] fix arm moving by 0 for unrestricted joints --- .../driver/kortex_arm_simulation.cpp | 27 +++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index e29606d2..c96aefdf 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -812,12 +812,35 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const ko // Transform kortex structure to trajectory_msgs to fill endpoint structure trajectory_msgs::JointTrajectoryPoint endpoint; + std::vector limited_joints; // joints limited in range + int degrees_of_freedom = constrained_joint_angles.joint_angles.joint_angles.size(); + if (degrees_of_freedom == 6) + { + limited_joints = {1,2,4}; + } + else if (degrees_of_freedom == 7) + { + limited_joints = {1,3,5}; + } + else + { + return FillKortexError(kortex_driver::ErrorCodes::ERROR_DEVICE, + kortex_driver::SubErrorCodes::INVALID_PARAM, + "Unsupported number of joints, expected 6 or 7"); + } for (int i = 0; i < constrained_joint_angles.joint_angles.joint_angles.size(); i++) { // If the current actuator has turned on itself many times, we need the endpoint to follow that trend too int n_turns = 0; - m_math_util.wrapRadiansFromMinusPiToPi(current.position[m_first_arm_joint_index + i], n_turns); - const double rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value)); + double rad_wrapped_goal; + if (std::count (limited_joints.begin(), limited_joints.end(), i)) + { + rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value)); + } + else + { + rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value), n_turns); + } endpoint.positions.push_back(rad_wrapped_goal + double(n_turns) * 2.0 * M_PI); endpoint.velocities.push_back(0.0); endpoint.accelerations.push_back(0.0); From 256e76039425e435010cc9b0fe4639d37bdb8f9b Mon Sep 17 00:00:00 2001 From: Felix Maisonneuve Date: Wed, 6 Oct 2021 15:32:59 -0400 Subject: [PATCH 2/2] use unordered set instead of vector --- .../src/non-generated/driver/kortex_arm_simulation.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp index c96aefdf..5a6f950c 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_simulation.cpp @@ -32,6 +32,7 @@ #include #include +#include namespace { @@ -812,7 +813,7 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const ko // Transform kortex structure to trajectory_msgs to fill endpoint structure trajectory_msgs::JointTrajectoryPoint endpoint; - std::vector limited_joints; // joints limited in range + std::unordered_set limited_joints; // joints limited in range int degrees_of_freedom = constrained_joint_angles.joint_angles.joint_angles.size(); if (degrees_of_freedom == 6) { @@ -833,7 +834,7 @@ kortex_driver::KortexError KortexArmSimulation::ExecuteReachJointAngles(const ko // If the current actuator has turned on itself many times, we need the endpoint to follow that trend too int n_turns = 0; double rad_wrapped_goal; - if (std::count (limited_joints.begin(), limited_joints.end(), i)) + if (limited_joints.count(i)) { rad_wrapped_goal = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(constrained_joint_angles.joint_angles.joint_angles[i].value)); }