From 191b3bea5d9d40f0d8df9e0cc32426fe0550e92d Mon Sep 17 00:00:00 2001 From: Felix Maisonneuve Date: Tue, 17 Aug 2021 13:00:30 -0400 Subject: [PATCH 1/2] move arm inbound on init using jointspeeds --- .../non-generated/kortex_arm_driver.h | 1 + .../non-generated/kortex_math_util.h | 3 +- .../driver/kortex_arm_driver.cpp | 64 +++++++++++++++++++ .../non-generated/driver/kortex_math_util.cpp | 12 ++++ 4 files changed, 79 insertions(+), 1 deletion(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h index 1c30e0f5..5f0ce28a 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_arm_driver.h @@ -80,6 +80,7 @@ class KortexArmDriver void initSubscribers(); void initRosServices(); void startActionServers(); + void moveArmWithinJointLimits(); private: diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h b/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h index 59b4c85d..1fc8517a 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h @@ -1,7 +1,7 @@ #ifndef _KORTEX_MATH_UTIL_H_ #define _KORTEX_MATH_UTIL_H_ -/* +/* * Copyright (c) 2019 Kinova inc. All rights reserved. * * This software may be modified and distributed under the @@ -29,6 +29,7 @@ class KortexMathUtil static double wrapDegreesFromZeroTo360(double deg_not_wrapped, int& number_of_turns); static double relative_position_from_absolute(double absolute_position, double min_value, double max_value); static double absolute_position_from_relative(double relative_position, double min_value, double max_value); + static double findDeltaFromBoundaries(double value, double limit); // kortex_driver::Twist helper functions static kortex_driver::Twist substractTwists(const kortex_driver::Twist& a, const kortex_driver::Twist& b); diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index 1e081e15..053952e1 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -49,6 +49,7 @@ KortexArmDriver::KortexArmDriver(ros::NodeHandle nh): m_node_handle(nh), if (m_is_real_robot) { m_publish_feedback_thread = std::thread(&KortexArmDriver::publishRobotFeedback, this); + moveArmWithinJointLimits(); } else { @@ -526,6 +527,69 @@ void KortexArmDriver::startActionServers() } } +void KortexArmDriver::moveArmWithinJointLimits() +{ + auto joint_angles = m_base->GetMeasuredJointAngles(); + + std::map limited_joints; + if (m_degrees_of_freedom == 6) + { + // We add angle limitations for joints 1,2 and 4 on 6 dof + limited_joints[1] = 128.9; + limited_joints[2] = 147.8; + limited_joints[4] = 120.3; + } + else + { + // We add angle limitations for joints 1,3 and 5 on 7 dof + limited_joints[1] = 128.9; + limited_joints[3] = 147.8; + limited_joints[5] = 120.3; + } + + Kinova::Api::Base::JointSpeeds joint_speeds = Kinova::Api::Base::JointSpeeds(); + Kinova::Api::Base::JointSpeed* joint_speed = joint_speeds.add_joint_speeds(); + + static const int TIME_COMPENSATION = 100; + static const int DEFAULT_JOINT_SPEED = 10; + static const int TIME_SPEED_RATIO = 1000 / DEFAULT_JOINT_SPEED; + + float angle; + for (unsigned int i = 0; i < m_degrees_of_freedom; i++) + { + angle = joint_angles.joint_angles(i).value(); + + // Angles received by GetMeasuredJointAngles are in range [0,360], but waypoints are in range [-180, 180] + angle = m_math_util.toDeg(m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(angle))); + + if(limited_joints.find(i) != limited_joints.end()) + { + float delta = m_math_util.findDeltaFromBoundaries(angle, limited_joints.at(i)); + + if (delta != 0) + { + // we add some time to compensate acceleration + int time_ms = delta * TIME_SPEED_RATIO + TIME_COMPENSATION; + int speed = DEFAULT_JOINT_SPEED; + joint_speed->set_joint_identifier(i); + + if (angle > 0) + { + speed *= -1; + } + + joint_speed->set_value(speed); + m_base->SendJointSpeedsCommand(joint_speeds); + + std::this_thread::sleep_for(std::chrono::milliseconds(time_ms)); + + joint_speed->set_value(0); + m_base->SendJointSpeedsCommand(joint_speeds); + } + } + } +} + bool KortexArmDriver::isGripperPresent() { return m_gripper_name != ""; diff --git a/kortex_driver/src/non-generated/driver/kortex_math_util.cpp b/kortex_driver/src/non-generated/driver/kortex_math_util.cpp index e89552bd..747c2ee6 100644 --- a/kortex_driver/src/non-generated/driver/kortex_math_util.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_math_util.cpp @@ -100,6 +100,18 @@ double KortexMathUtil::absolute_position_from_relative(double relative_position, return relative_position * range + min_value; } +double KortexMathUtil::findDeltaFromBoundaries(double value, double limit) +{ + double delta = std::abs(value) - limit; + + if ( delta < 0 ) + { + delta = 0; + } + + return delta; +} + kortex_driver::Twist KortexMathUtil::substractTwists(const kortex_driver::Twist& a, const kortex_driver::Twist& b) { kortex_driver::Twist c; From 6a1c68179c556e4e8c70c700cb36e23c9a148a1a Mon Sep 17 00:00:00 2001 From: Felix Maisonneuve Date: Mon, 23 Aug 2021 10:15:58 -0400 Subject: [PATCH 2/2] update based on PR feedback --- .../non-generated/kortex_math_util.h | 2 +- .../non-generated/driver/kortex_arm_driver.cpp | 18 ++++++++++++------ .../non-generated/driver/kortex_math_util.cpp | 10 +++++----- 3 files changed, 18 insertions(+), 12 deletions(-) diff --git a/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h b/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h index 1fc8517a..6a26ece9 100644 --- a/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h +++ b/kortex_driver/include/kortex_driver/non-generated/kortex_math_util.h @@ -29,7 +29,7 @@ class KortexMathUtil static double wrapDegreesFromZeroTo360(double deg_not_wrapped, int& number_of_turns); static double relative_position_from_absolute(double absolute_position, double min_value, double max_value); static double absolute_position_from_relative(double relative_position, double min_value, double max_value); - static double findDeltaFromBoundaries(double value, double limit); + static float findDistanceToBoundary(float value, float limit); // kortex_driver::Twist helper functions static kortex_driver::Twist substractTwists(const kortex_driver::Twist& a, const kortex_driver::Twist& b); diff --git a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp index 053952e1..e2a70f24 100644 --- a/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp @@ -534,18 +534,23 @@ void KortexArmDriver::moveArmWithinJointLimits() std::map limited_joints; if (m_degrees_of_freedom == 6) { - // We add angle limitations for joints 1,2 and 4 on 6 dof + // We add angle limitations for joints 1,2 and 4 on 6 dof (values from User guide) limited_joints[1] = 128.9; limited_joints[2] = 147.8; limited_joints[4] = 120.3; } - else + else if (m_degrees_of_freedom == 7) { - // We add angle limitations for joints 1,3 and 5 on 7 dof + // We add angle limitations for joints 1,3 and 5 on 7 dof (values from User guide) limited_joints[1] = 128.9; limited_joints[3] = 147.8; limited_joints[5] = 120.3; } + else + { + ROS_WARN("Unsupported number of actuators. Not moving the arm within joint limits"); + return; + } Kinova::Api::Base::JointSpeeds joint_speeds = Kinova::Api::Base::JointSpeeds(); Kinova::Api::Base::JointSpeed* joint_speed = joint_speeds.add_joint_speeds(); @@ -553,6 +558,7 @@ void KortexArmDriver::moveArmWithinJointLimits() static const int TIME_COMPENSATION = 100; static const int DEFAULT_JOINT_SPEED = 10; static const int TIME_SPEED_RATIO = 1000 / DEFAULT_JOINT_SPEED; + static const float EPSILON = 0.001; float angle; for (unsigned int i = 0; i < m_degrees_of_freedom; i++) @@ -562,11 +568,11 @@ void KortexArmDriver::moveArmWithinJointLimits() // Angles received by GetMeasuredJointAngles are in range [0,360], but waypoints are in range [-180, 180] angle = m_math_util.toDeg(m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(angle))); - if(limited_joints.find(i) != limited_joints.end()) + if (limited_joints.count(i)) { - float delta = m_math_util.findDeltaFromBoundaries(angle, limited_joints.at(i)); + float delta = m_math_util.findDistanceToBoundary(angle, limited_joints.at(i)); - if (delta != 0) + if (delta > EPSILON) { // we add some time to compensate acceleration int time_ms = delta * TIME_SPEED_RATIO + TIME_COMPENSATION; diff --git a/kortex_driver/src/non-generated/driver/kortex_math_util.cpp b/kortex_driver/src/non-generated/driver/kortex_math_util.cpp index 747c2ee6..0f219925 100644 --- a/kortex_driver/src/non-generated/driver/kortex_math_util.cpp +++ b/kortex_driver/src/non-generated/driver/kortex_math_util.cpp @@ -100,16 +100,16 @@ double KortexMathUtil::absolute_position_from_relative(double relative_position, return relative_position * range + min_value; } -double KortexMathUtil::findDeltaFromBoundaries(double value, double limit) +float KortexMathUtil::findDistanceToBoundary(float value, float limit) { - double delta = std::abs(value) - limit; + float distance = std::abs(value) - limit; - if ( delta < 0 ) + if ( distance < 0 ) { - delta = 0; + distance = 0; } - return delta; + return distance; } kortex_driver::Twist KortexMathUtil::substractTwists(const kortex_driver::Twist& a, const kortex_driver::Twist& b)