From 5511776beddb0d7609db704c373b5dd3fe54a09d Mon Sep 17 00:00:00 2001 From: Felix Maisonneuve Date: Wed, 11 Aug 2021 13:57:08 -0400 Subject: [PATCH 1/2] add changes from melodic-devel PRs --- kortex_driver/CMakeLists.txt | 6 +- ...ple_cartesian_poses_with_notifications.cpp | 2 - .../full_arm/example_full_arm_movement.cpp | 112 ++++++++++++------ .../src/full_arm/example_full_arm_movement.py | 109 +++++++++++------ kortex_gazebo/readme.md | 2 +- third_party/gazebo-pkgs/README.md | 2 +- .../README.md | 2 +- 7 files changed, 160 insertions(+), 75 deletions(-) diff --git a/kortex_driver/CMakeLists.txt b/kortex_driver/CMakeLists.txt index 58ddb307..1b3cb4bf 100644 --- a/kortex_driver/CMakeLists.txt +++ b/kortex_driver/CMakeLists.txt @@ -80,21 +80,21 @@ if(USE_CONAN) BASIC_SETUP CMAKE_TARGETS NO_OUTPUT_DIRS SETTINGS kortex_api_cpp:arch=armv7 kortex_api_cpp:compiler=gcc kortex_api_cpp:compiler.version=5 kortex_api_cpp:compiler.libcxx=libstdc++11 - OPTIONS kortex_api_cpp:target=artik710) + ENV TARGET=artik710) elseif("${CONAN_TARGET_PLATFORM}" STREQUAL "imx6") conan_cmake_run(CONANFILE conanfile.py UPDATE BASIC_SETUP CMAKE_TARGETS NO_OUTPUT_DIRS SETTINGS kortex_api_cpp:arch=armv7 kortex_api_cpp:compiler=gcc kortex_api_cpp:compiler.version=6.4 kortex_api_cpp:compiler.libcxx=libstdc++11 - OPTIONS kortex_api_cpp:target=imx6) + ENV TARGET=imx6) elseif("${CONAN_TARGET_PLATFORM}" STREQUAL "jetson") conan_cmake_run(CONANFILE conanfile.py UPDATE BASIC_SETUP CMAKE_TARGETS NO_OUTPUT_DIRS SETTINGS kortex_api_cpp:arch=armv7 kortex_api_cpp:compiler=gcc kortex_api_cpp:compiler.version=7 kortex_api_cpp:compiler.libcxx=libstdc++11 - OPTIONS kortex_api_cpp:target=jetson) + ENV TARGET=jetson) endif() endif() diff --git a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp index e9d7406f..d64f5002 100644 --- a/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp +++ b/kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp @@ -14,12 +14,10 @@ #include "ros/ros.h" #include -#include #include #include #include #include -#include #include #include #include diff --git a/kortex_examples/src/full_arm/example_full_arm_movement.cpp b/kortex_examples/src/full_arm/example_full_arm_movement.cpp index edcd474b..35ff3dc4 100644 --- a/kortex_examples/src/full_arm/example_full_arm_movement.cpp +++ b/kortex_examples/src/full_arm/example_full_arm_movement.cpp @@ -15,12 +15,10 @@ #include "ros/ros.h" #include -#include #include #include #include #include -#include #include #include #include @@ -29,6 +27,7 @@ #include #include #include +#include #include #include @@ -161,9 +160,11 @@ bool example_send_cartesian_pose(ros::NodeHandle n, const std::string &robot_nam // Here we only need the latest message in the topic though auto feedback = ros::topic::waitForMessage("/" + robot_name + "/base_feedback"); - // Initialize the ServiceClient for the cartesian pose - ros::ServiceClient service_client_play_cartesian_trajectory = n.serviceClient("/" + robot_name + "/base/play_cartesian_trajectory"); - kortex_driver::PlayCartesianTrajectory service_play_cartesian_trajectory; + // Initialize the ServiceClient + ros::ServiceClient service_client_execute_waypoints_trajectory = n.serviceClient("/" + robot_name + "/base/execute_waypoint_trajectory"); + kortex_driver::ExecuteWaypointTrajectory service_execute_waypoints_trajectory; + + kortex_driver::Waypoint waypoint; // Initialize input float current_x = feedback->base.commanded_tool_pose_x; @@ -174,28 +175,18 @@ bool example_send_cartesian_pose(ros::NodeHandle n, const std::string &robot_nam float current_theta_z = feedback->base.commanded_tool_pose_theta_z; // Creating the target pose - service_play_cartesian_trajectory.request.input.target_pose.x = current_x; - service_play_cartesian_trajectory.request.input.target_pose.y = current_y; - service_play_cartesian_trajectory.request.input.target_pose.z = current_z + 0.10; - service_play_cartesian_trajectory.request.input.target_pose.theta_x = current_theta_x; - service_play_cartesian_trajectory.request.input.target_pose.theta_y = current_theta_y; - service_play_cartesian_trajectory.request.input.target_pose.theta_z = current_theta_z; - - kortex_driver::CartesianSpeed poseSpeed; - poseSpeed.translation = 0.1; - poseSpeed.orientation = 15; - - // The constraint is a one_of in Protobuf. The one_of concept does not exist in ROS - // To specify a one_of, create it and put it in the appropriate vector of the oneof_type member of the ROS object : - service_play_cartesian_trajectory.request.input.constraint.oneof_type.speed.push_back(poseSpeed); - - if (service_client_play_cartesian_trajectory.call(service_play_cartesian_trajectory)) + service_execute_waypoints_trajectory.request.input.waypoints.push_back(FillCartesianWaypoint(current_x, current_y, current_z + 0.10, current_theta_x, current_theta_y, current_theta_z, 0)); + + service_execute_waypoints_trajectory.request.input.duration = 0; + service_execute_waypoints_trajectory.request.input.use_optimal_blending = false; + + if (service_client_execute_waypoints_trajectory.call(service_execute_waypoints_trajectory)) { ROS_INFO("The new cartesian pose was sent to the robot."); } else { - std::string error_string = "Failed to call PlayCartesianTrajectory"; + std::string error_string = "Failed to call ExecuteWaypointTrajectory"; ROS_ERROR("%s", error_string.c_str()); return false; } @@ -207,30 +198,79 @@ bool example_send_joint_angles(ros::NodeHandle n, const std::string &robot_name, { last_action_notification_event = 0; // Initialize the ServiceClient - ros::ServiceClient service_client_play_joint_trajectory = n.serviceClient("/" + robot_name + "/base/play_joint_trajectory"); - kortex_driver::PlayJointTrajectory service_play_joint_trajectory; + ros::ServiceClient service_client_execute_waypoints_trajectory = n.serviceClient("/" + robot_name + "/base/execute_waypoint_trajectory"); + kortex_driver::ExecuteWaypointTrajectory service_execute_waypoints_trajectory; + + ros::ServiceClient service_client_validate_waypoint_list = n.serviceClient("/" + robot_name + "/base/validate_waypoint_list"); + kortex_driver::ValidateWaypointList service_validate_waypoint_list; + + kortex_driver::WaypointList trajectory; + kortex_driver::Waypoint waypoint; + kortex_driver::AngularWaypoint angularWaypoint; - std::vector angles_to_send; + // Angles to send the arm to vertical position (all zeros) for (unsigned int i = 0; i < degrees_of_freedom; i++) { - angles_to_send.push_back(0.0); + angularWaypoint.angles.push_back(0.0); + } + + // Each AngularWaypoint needs a duration and the global duration (from WaypointList) is disregarded. + // If you put 0 to the global duration, the trajectory will be optimal. + // If you somehting too small (for either global duration or AngularWaypoint duration), the trajectory will be rejected. + int angular_duration = 0; + angularWaypoint.duration = angular_duration; + + // Initialize Waypoint and WaypointList + waypoint.oneof_type_of_waypoint.angular_waypoint.push_back(angularWaypoint); + trajectory.duration = 0; + trajectory.use_optimal_blending = false; + trajectory.waypoints.push_back(waypoint); + + service_validate_waypoint_list.request.input = trajectory; + if (!service_client_validate_waypoint_list.call(service_validate_waypoint_list)) + { + std::string error_string = "Failed to call ValidateWaypointList"; + ROS_ERROR("%s", error_string.c_str()); + return false; + } + + int error_number = service_validate_waypoint_list.response.output.trajectory_error_report.trajectory_error_elements.size(); + static const int MAX_ANGULAR_DURATION = 30; + + while (error_number >= 1 && angular_duration < MAX_ANGULAR_DURATION) + { + angular_duration++; + trajectory.waypoints[0].oneof_type_of_waypoint.angular_waypoint[0].duration = angular_duration; + + service_validate_waypoint_list.request.input = trajectory; + if (!service_client_validate_waypoint_list.call(service_validate_waypoint_list)) + { + std::string error_string = "Failed to call ValidateWaypointList"; + ROS_ERROR("%s", error_string.c_str()); + return false; + } + error_number = service_validate_waypoint_list.response.output.trajectory_error_report.trajectory_error_elements.size(); } - for (int i = 0; i < degrees_of_freedom; i++) + if (angular_duration >= MAX_ANGULAR_DURATION) { - kortex_driver::JointAngle temp_angle; - temp_angle.joint_identifier = i; - temp_angle.value = angles_to_send[i]; - service_play_joint_trajectory.request.input.joint_angles.joint_angles.push_back(temp_angle); + // It should be possible to reach position within 30s + // WaypointList is invalid (other error than angularWaypoint duration) + std::string error_string = "WaypointList is invalid"; + ROS_ERROR("%s", error_string.c_str()); + return false; } - if (service_client_play_joint_trajectory.call(service_play_joint_trajectory)) + service_execute_waypoints_trajectory.request.input = trajectory; + + // Send the angles + if (service_client_execute_waypoints_trajectory.call(service_execute_waypoints_trajectory)) { ROS_INFO("The joint angles were sent to the robot."); } else { - std::string error_string = "Failed to call PlayJointTrajectory"; + std::string error_string = "Failed to call ExecuteWaypointTrajectory"; ROS_ERROR("%s", error_string.c_str()); return false; } @@ -276,7 +316,11 @@ bool example_cartesian_waypoint(ros::NodeHandle n, const std::string &robot_name last_action_notification_event = 0; - if (!service_client_get_config.call(service_get_config)) + if (service_client_execute_waypoints_trajectory.call(service_execute_waypoints_trajectory)) + { + ROS_INFO("The WaypointList command was sent to the robot."); + } + else { std::string error_string = "Failed to call GetProductConfiguration"; ROS_ERROR("%s", error_string.c_str()); diff --git a/kortex_examples/src/full_arm/example_full_arm_movement.py b/kortex_examples/src/full_arm/example_full_arm_movement.py index 9bd6a4f5..7b24b089 100755 --- a/kortex_examples/src/full_arm/example_full_arm_movement.py +++ b/kortex_examples/src/full_arm/example_full_arm_movement.py @@ -54,14 +54,6 @@ def __init__(self): rospy.wait_for_service(set_cartesian_reference_frame_full_name) self.set_cartesian_reference_frame = rospy.ServiceProxy(set_cartesian_reference_frame_full_name, SetCartesianReferenceFrame) - play_cartesian_trajectory_full_name = '/' + self.robot_name + '/base/play_cartesian_trajectory' - rospy.wait_for_service(play_cartesian_trajectory_full_name) - self.play_cartesian_trajectory = rospy.ServiceProxy(play_cartesian_trajectory_full_name, PlayCartesianTrajectory) - - play_joint_trajectory_full_name = '/' + self.robot_name + '/base/play_joint_trajectory' - rospy.wait_for_service(play_joint_trajectory_full_name) - self.play_joint_trajectory = rospy.ServiceProxy(play_joint_trajectory_full_name, PlayJointTrajectory) - send_gripper_command_full_name = '/' + self.robot_name + '/base/send_gripper_command' rospy.wait_for_service(send_gripper_command_full_name) self.send_gripper_command = rospy.ServiceProxy(send_gripper_command_full_name, SendGripperCommand) @@ -73,6 +65,10 @@ def __init__(self): get_product_configuration_full_name = '/' + self.robot_name + '/base/get_product_configuration' rospy.wait_for_service(get_product_configuration_full_name) self.get_product_configuration = rospy.ServiceProxy(get_product_configuration_full_name, GetProductConfiguration) + + validate_waypoint_list_full_name = '/' + self.robot_name + '/base/validate_waypoint_list' + rospy.wait_for_service(validate_waypoint_list_full_name) + self.validate_waypoint_list = rospy.ServiceProxy(validate_waypoint_list_full_name, ValidateWaypointList) except: self.is_init_success = False else: @@ -184,49 +180,96 @@ def example_send_cartesian_pose(self): # Here we only need the latest message in the topic though feedback = rospy.wait_for_message("/" + self.robot_name + "/base_feedback", BaseCyclic_Feedback) - req = PlayCartesianTrajectoryRequest() - req.input.target_pose.x = feedback.base.commanded_tool_pose_x - req.input.target_pose.y = feedback.base.commanded_tool_pose_y - req.input.target_pose.z = feedback.base.commanded_tool_pose_z + 0.10 - req.input.target_pose.theta_x = feedback.base.commanded_tool_pose_theta_x - req.input.target_pose.theta_y = feedback.base.commanded_tool_pose_theta_y - req.input.target_pose.theta_z = feedback.base.commanded_tool_pose_theta_z + # Possible to execute waypointList via execute_action service or use execute_waypoint_trajectory service directly + req = ExecuteActionRequest() + trajectory = WaypointList() + + trajectory.waypoints.append( + self.FillCartesianWaypoint( + feedback.base.commanded_tool_pose_x, + feedback.base.commanded_tool_pose_y, + feedback.base.commanded_tool_pose_z + 0.10, + feedback.base.commanded_tool_pose_theta_x, + feedback.base.commanded_tool_pose_theta_y, + feedback.base.commanded_tool_pose_theta_z, + 0) + ) - pose_speed = CartesianSpeed() - pose_speed.translation = 0.1 - pose_speed.orientation = 15 + trajectory.duration = 0 + trajectory.use_optimal_blending = False - # The constraint is a one_of in Protobuf. The one_of concept does not exist in ROS - # To specify a one_of, create it and put it in the appropriate list of the oneof_type member of the ROS object : - req.input.constraint.oneof_type.speed.append(pose_speed) + req.input.oneof_action_parameters.execute_waypoint_list.append(trajectory) # Call the service rospy.loginfo("Sending the robot to the cartesian pose...") try: - self.play_cartesian_trajectory(req) + self.execute_action(req) except rospy.ServiceException: - rospy.logerr("Failed to call PlayCartesianTrajectory") + rospy.logerr("Failed to call ExecuteWaypointTrajectory") return False else: return self.wait_for_action_end_or_abort() def example_send_joint_angles(self): self.last_action_notif_type = None - # Create the list of angles - req = PlayJointTrajectoryRequest() - # Here the arm is vertical (all zeros) - for i in range(self.degrees_of_freedom): - temp_angle = JointAngle() - temp_angle.joint_identifier = i - temp_angle.value = 0.0 - req.input.joint_angles.joint_angles.append(temp_angle) + + req = ExecuteActionRequest() + + trajectory = WaypointList() + waypoint = Waypoint() + angularWaypoint = AngularWaypoint() + + # Angles to send the arm to vertical position (all zeros) + for _ in range(self.degrees_of_freedom): + angularWaypoint.angles.append(0.0) + + # Each AngularWaypoint needs a duration and the global duration (from WaypointList) is disregarded. + # If you put 0 to the global duration, the trajectory will be optimal. + # If you somehting too small (for either global duration or AngularWaypoint duration), the trajectory will be rejected. + angular_duration = 0 + angularWaypoint.duration = angular_duration + + # Initialize Waypoint and WaypointList + waypoint.oneof_type_of_waypoint.angular_waypoint.append(angularWaypoint) + trajectory.duration = 0 + trajectory.use_optimal_blending = False + trajectory.waypoints.append(waypoint) + + try: + res = self.validate_waypoint_list(trajectory) + except rospy.ServiceException: + rospy.logerr("Failed to call ValidateWaypointList") + return False + + error_number = len(res.output.trajectory_error_report.trajectory_error_elements) + MAX_ANGULAR_DURATION = 30 + + while (error_number >= 1 and angular_duration != MAX_ANGULAR_DURATION) : + angular_duration += 1 + trajectory.waypoints[0].oneof_type_of_waypoint.angular_waypoint[0].duration = angular_duration + + try: + res = self.validate_waypoint_list(trajectory) + except rospy.ServiceException: + rospy.logerr("Failed to call ValidateWaypointList") + return False + + error_number = len(res.output.trajectory_error_report.trajectory_error_elements) + + if (angular_duration == MAX_ANGULAR_DURATION) : + # It should be possible to reach position within 30s + # WaypointList is invalid (other error than angularWaypoint duration) + rospy.loginfo("WaypointList is invalid") + return False + + req.input.oneof_action_parameters.execute_waypoint_list.append(trajectory) # Send the angles rospy.loginfo("Sending the robot vertical...") try: - self.play_joint_trajectory(req) + self.execute_action(req) except rospy.ServiceException: - rospy.logerr("Failed to call PlayJointTrajectory") + rospy.logerr("Failed to call ExecuteWaypointjectory") return False else: return self.wait_for_action_end_or_abort() diff --git a/kortex_gazebo/readme.md b/kortex_gazebo/readme.md index 68c2cf3a..e530d742 100644 --- a/kortex_gazebo/readme.md +++ b/kortex_gazebo/readme.md @@ -52,7 +52,7 @@ The launch can be parametrized with arguments : - **y0** : The default Y-axis position of the robot in Gazebo. The default value is **0.0**. - **z0** : The default Z-axis position of the robot in Gazebo. The default value is **0.0**. - **arm** : Name of your robot arm model. See the `kortex_description/arms` folder to see the available robot models. The default value is **gen3**. -- **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""**. For Gen3, you can also put **robotiq_2f_85**. For Gen3 lite, you need to put **gen3_lite_2f**. +- **gripper** : Name of your robot arm's tool / gripper. See the `kortex_description/grippers` folder to see the available end effector models (or to add your own). The default value is **""**. For Gen3, you can also put **robotiq_2f_85** or **robotiq_2f_140**. For Gen3 lite, you need to put **gen3_lite_2f**. - **robot_name** : This is the namespace of the arm that is going to be spawned. It defaults to **my_$(arg arm)** (so my_gen3 for arm="gen3"). - **use_trajectory_controller** : If this argument is false, one `JointPositionController` per joint will be launched and the arm will offer a basic ROS Control interface to control every joint individually with topics. If this argument is true, a MoveIt! node will be started for the arm and the arm will offer a `FollowJointTrajectory` interface to control the arm (via a `JointTrajectoryController`). The default value is **true**. - **use_sim_time** : If this value is true, Gazebo will use simulated time instead of system clock. The default value is **true**. diff --git a/third_party/gazebo-pkgs/README.md b/third_party/gazebo-pkgs/README.md index 5cbf8493..f409496f 100644 --- a/third_party/gazebo-pkgs/README.md +++ b/third_party/gazebo-pkgs/README.md @@ -1,4 +1,4 @@ -These ROS packages were cloned from https://github.com/JenniferBuehler/gazebo-pkgs into ros_kortex to properly simulate grasping in Gazebo for the Robotiq 2f 85 gripper. +These ROS packages were cloned from https://github.com/JenniferBuehler/gazebo-pkgs into ros_kortex to properly simulate grasping in Gazebo for the Robotiq 2f 85 gripper and the Robotiq 2f 140 gripper. The repository was cloned at commit a7ebecca4393d43393e315d379a876e71820fd96. note: Some packages were removed since they weren't needed. diff --git a/third_party/roboticsgroup_upatras_gazebo_plugins/README.md b/third_party/roboticsgroup_upatras_gazebo_plugins/README.md index c4808cbc..d467a3ea 100644 --- a/third_party/roboticsgroup_upatras_gazebo_plugins/README.md +++ b/third_party/roboticsgroup_upatras_gazebo_plugins/README.md @@ -1,4 +1,4 @@ -This ROS package was cloned from https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins into ros_kortex to properly simulate mimic joints in Gazebo for the Robotiq 2f 85 gripper. +This ROS package was cloned from https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins into ros_kortex to properly simulate mimic joints in Gazebo for the Robotiq 2f 85 gripper the Robotiq 2f 140 gripper. The repository was cloned at commit 15187abc17f22dbd3fa720d70ad50de74a668b7c. Originally, for ROS Melodic and Kinetic, we used the ROS package from https://github.com/roboticsgroup/roboticsgroup_gazebo_plugins at commit : 4d93ecd86e4415c3fe74d0027fd41c5e3b39ec44. From c11af0fe3708f67e662e6899aa95379013654eab Mon Sep 17 00:00:00 2001 From: Felix Maisonneuve Date: Mon, 16 Aug 2021 10:43:24 -0400 Subject: [PATCH 2/2] fix wrong copy-paste + missing word and typo in comment --- .../src/full_arm/example_full_arm_movement.cpp | 15 +++++++-------- .../src/full_arm/example_full_arm_movement.py | 3 +-- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/kortex_examples/src/full_arm/example_full_arm_movement.cpp b/kortex_examples/src/full_arm/example_full_arm_movement.cpp index 35ff3dc4..82daaf8d 100644 --- a/kortex_examples/src/full_arm/example_full_arm_movement.cpp +++ b/kortex_examples/src/full_arm/example_full_arm_movement.cpp @@ -215,8 +215,7 @@ bool example_send_joint_angles(ros::NodeHandle n, const std::string &robot_name, } // Each AngularWaypoint needs a duration and the global duration (from WaypointList) is disregarded. - // If you put 0 to the global duration, the trajectory will be optimal. - // If you somehting too small (for either global duration or AngularWaypoint duration), the trajectory will be rejected. + // If you put something too small (for either global duration or AngularWaypoint duration), the trajectory will be rejected. int angular_duration = 0; angularWaypoint.duration = angular_duration; @@ -316,11 +315,7 @@ bool example_cartesian_waypoint(ros::NodeHandle n, const std::string &robot_name last_action_notification_event = 0; - if (service_client_execute_waypoints_trajectory.call(service_execute_waypoints_trajectory)) - { - ROS_INFO("The WaypointList command was sent to the robot."); - } - else + if (!service_client_get_config.call(service_get_config)) { std::string error_string = "Failed to call GetProductConfiguration"; ROS_ERROR("%s", error_string.c_str()); @@ -350,7 +345,11 @@ bool example_cartesian_waypoint(ros::NodeHandle n, const std::string &robot_name service_execute_waypoints_trajectory.request.input.duration = 0; service_execute_waypoints_trajectory.request.input.use_optimal_blending = 0; - if (!service_client_execute_waypoints_trajectory.call(service_execute_waypoints_trajectory)) + if (service_client_execute_waypoints_trajectory.call(service_execute_waypoints_trajectory)) + { + ROS_INFO("The WaypointList command was sent to the robot."); + } + else { std::string error_string = "Failed to call ExecuteWaypointTrajectory"; ROS_ERROR("%s", error_string.c_str()); diff --git a/kortex_examples/src/full_arm/example_full_arm_movement.py b/kortex_examples/src/full_arm/example_full_arm_movement.py index 7b24b089..2aae0531 100755 --- a/kortex_examples/src/full_arm/example_full_arm_movement.py +++ b/kortex_examples/src/full_arm/example_full_arm_movement.py @@ -224,8 +224,7 @@ def example_send_joint_angles(self): angularWaypoint.angles.append(0.0) # Each AngularWaypoint needs a duration and the global duration (from WaypointList) is disregarded. - # If you put 0 to the global duration, the trajectory will be optimal. - # If you somehting too small (for either global duration or AngularWaypoint duration), the trajectory will be rejected. + # If you put something too small (for either global duration or AngularWaypoint duration), the trajectory will be rejected. angular_duration = 0 angularWaypoint.duration = angular_duration