Merge pull request #183 from Kinovarobotics/opened-pull-requests-noetic

Opened pull requests noetic

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()
 

kortex_examples/src/full_arm/example_cartesian_poses_with_notifications.cpp

@@ -14,12 +14,10 @@
 
 #include "ros/ros.h"
 #include <kortex_driver/Base_ClearFaults.h>
-#include <kortex_driver/PlayCartesianTrajectory.h>
 #include <kortex_driver/CartesianSpeed.h>
 #include <kortex_driver/BaseCyclic_Feedback.h>
 #include <kortex_driver/ReadAction.h>
 #include <kortex_driver/ExecuteAction.h>
-#include <kortex_driver/PlayJointTrajectory.h>
 #include <kortex_driver/SetCartesianReferenceFrame.h>
 #include <kortex_driver/CartesianReferenceFrame.h>
 #include <kortex_driver/SendGripperCommand.h>

kortex_examples/src/full_arm/example_full_arm_movement.cpp

@@ -15,12 +15,10 @@
 #include "ros/ros.h"
 
 #include <kortex_driver/Base_ClearFaults.h>
-#include <kortex_driver/PlayCartesianTrajectory.h>
 #include <kortex_driver/CartesianSpeed.h>
 #include <kortex_driver/BaseCyclic_Feedback.h>
 #include <kortex_driver/ReadAction.h>
 #include <kortex_driver/ExecuteAction.h>
-#include <kortex_driver/PlayJointTrajectory.h>
 #include <kortex_driver/SetCartesianReferenceFrame.h>
 #include <kortex_driver/CartesianReferenceFrame.h>
 #include <kortex_driver/SendGripperCommand.h>
@@ -29,6 +27,7 @@
 #include <kortex_driver/ActionEvent.h>
 #include <kortex_driver/OnNotificationActionTopic.h>
 #include <kortex_driver/ExecuteWaypointTrajectory.h>
+#include <kortex_driver/ValidateWaypointList.h>
 #include <kortex_driver/GetProductConfiguration.h>
 #include <kortex_driver/ModelId.h>
 
@@ -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<kortex_driver::BaseCyclic_Feedback>("/" + robot_name + "/base_feedback");
 
-  // Initialize the ServiceClient for the cartesian pose
-  ros::ServiceClient service_client_play_cartesian_trajectory = n.serviceClient<kortex_driver::PlayCartesianTrajectory>("/" + 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<kortex_driver::ExecuteWaypointTrajectory>("/" + 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,78 @@ 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<kortex_driver::PlayJointTrajectory>("/" + robot_name + "/base/play_joint_trajectory");
-  kortex_driver::PlayJointTrajectory service_play_joint_trajectory;
+  ros::ServiceClient service_client_execute_waypoints_trajectory = n.serviceClient<kortex_driver::ExecuteWaypointTrajectory>("/" + robot_name + "/base/execute_waypoint_trajectory");
+  kortex_driver::ExecuteWaypointTrajectory service_execute_waypoints_trajectory;
+
+  ros::ServiceClient service_client_validate_waypoint_list = n.serviceClient<kortex_driver::ValidateWaypointList>("/" + 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<double> 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 something 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;
   }
@@ -306,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());

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,95 @@ 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 something 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()

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**.

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.