change gripper control from moveit to ros2_control

rasmt_moveit_config/config/ompl_planning.yaml

@@ -154,8 +154,7 @@ rasmt_arm_group:
     - LazyPRMstar
     - SPARS
     - SPARStwo
-  projection_evaluator: joints(rasmt_Rot_Z_1,rasmt_Rot_Y_1)
+
-  longest_valid_segment_fraction: 0.005
 rasmt_hand_arm_group:
   planner_configs:
     - AnytimePathShortening

rasmt_moveit_config/config/rasmt.srdf

@@ -10,15 +10,15 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="rasmt_arm_group">
-        <chain base_link="rasmt_Base_Link" tip_link="rasmt_tool_end_point"/>
+        <chain base_link="rasmt_Base_Link" tip_link="rasmt_tool0"/>
     </group>
-    <group name="rasmt_hand_arm_group">
+    <!--group name="rasmt_hand_arm_group">
         <link name="rasmt_Grip_Body"/>
         <link name="rasmt_Grip_L"/>
         <link name="rasmt_Grip_R"/>
         <joint name="rasmt_Slide_1"/>
         <joint name="rasmt_Slide_2"/>
-    </group>
+    </group-->
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="home_position" group="rasmt_arm_group">
         <joint name="rasmt_Rot_Y_1" value="0"/>
@@ -44,7 +44,7 @@
         <joint name="rasmt_Rot_Z_2" value="3.1416"/>
         <joint name="rasmt_Rot_Z_3" value="0.0521"/>
     </group_state>
-    <group_state name="home_hand" group="rasmt_hand_arm_group">
+    <!--group_state name="home_hand" group="rasmt_hand_arm_group">
         <joint name="rasmt_Slide_1" value="0.02"/>
         <joint name="rasmt_Slide_2" value="0.02"/>
     </group_state>
@@ -55,7 +55,7 @@
     <group_state name="full_close" group="rasmt_hand_arm_group">
         <joint name="rasmt_Slide_1" value="0.0"/>
         <joint name="rasmt_Slide_2" value="0.0"/>
-    </group_state>
+    </group_state-->
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <!--end_effector name="rasmt_hand" parent_link="rasmt_Dock_Link" group="rasmt_hand_arm_group" parent_group="rasmt_arm_group"/-->
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->

rasmt_moveit_config/config/rasmt_moveit.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
       Splitter Ratio: 0.5
-    Tree Height: 787
+    Tree Height: 389
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -43,7 +43,7 @@ Visualization Manager:
       Reference Frame: <Fixed Frame>
       Value: true
     - Class: moveit_rviz_plugin/PlanningScene
-      Enabled: true
+      Enabled: false
       Move Group Namespace: ""
       Name: PlanningScene
       Planning Scene Topic: /monitored_planning_scene
@@ -128,10 +128,10 @@ Visualization Manager:
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
-      Value: true
+      Value: false
     - Acceleration_Scaling_Factor: 1
       Class: moveit_rviz_plugin/MotionPlanning
-      Enabled: false
+      Enabled: true
       Move Group Namespace: ""
       MoveIt_Allow_Approximate_IK: false
       MoveIt_Allow_External_Program: false
@@ -334,7 +334,7 @@ Visualization Manager:
         Robot Alpha: 1
         Show Robot Collision: false
         Show Robot Visual: true
-      Value: false
+      Value: true
       Velocity_Scaling_Factor: 1
   Enabled: true
   Global Options:
@@ -409,7 +409,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000025e0000017d0000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f30000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000210000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000253000001880000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000053f0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
@@ -417,5 +417,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1848
-  X: 520
+  X: 72
-  Y: 200
+  Y: 27

rasmt_moveit_config/config/rasmt_moveit_controllers.yaml

@@ -1,6 +1,6 @@
 controller_names:
   - rasmt_arm_controller
-  - rasmt_hand_controller
+#  - rasmt_hand_controller
 
 rasmt_arm_controller:
   action_ns: follow_joint_trajectory
@@ -14,10 +14,10 @@ rasmt_arm_controller:
       - rasmt_Rot_Z_3
       - rasmt_Rot_Y_4
 
-rasmt_hand_controller:
+#rasmt_hand_controller:
-  action_ns: follow_joint_trajectory
+#  action_ns: follow_joint_trajectory
-  type: FollowJointTrajectory
+#  type: FollowJointTrajectory
-  default: true
+#  default: true
-  joints:
+#  joints:
-    - rasmt_Slide_1
+#    - rasmt_Slide_1
-    - rasmt_Slide_2
+#    - rasmt_Slide_2

rasmt_support/config/rasmt_ros2_controllers.yaml

@@ -6,7 +6,7 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
 
     rasmt_hand_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
+      type: effort_controllers/JointGroupEffortController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
@@ -33,7 +33,8 @@ rasmt_hand_controller:
       - rasmt_Slide_1
       - rasmt_Slide_2
     command_interfaces:
-      - position
+      - effort
     state_interfaces:
       - position
-      - velocity
+      - velocity
+      - effort

rasmt_support/urdf/robot/rasmt_single_macro.xacro

@@ -409,13 +409,13 @@
       <axis xyz="0 0 0"/>
   </joint>
 
-  <link name="${prefix}_tool_end_point"/>
+  <!--link name="${prefix}_tool_end_point"/>
   <joint name="${prefix}_tool_end_point_to_tool0" type="fixed">
-      <parent link="${prefix}_tool0"/>
+      <parent link="${prefix}_Dock_Link"/>
       <child link="${prefix}_tool_end_point"/>
-      <origin xyz="0 0 ${gripper_length}" rpy="0 0 0"/>
+      <origin xyz="0 0 ${gripper_length+0.12324}" rpy="0 0 0"/>
       <axis xyz="0 0 0"/>
-  </joint>
+  </joint-->
 
   </xacro:macro>
 </robot>

rasmt_support/urdf/tools/rasmt_hand_control.xacro

@@ -17,10 +17,10 @@
             </xacro:unless>
 
             <joint name="${prefix}_Slide_1">
-                <command_interface name="position">
+                <command_interface name="effort">
                     <!-- better to use radians as min max first -->
-                    <param name="min">-1</param>
+                    <param name="min">-10</param>
-                    <param name="max"> 1</param>
+                    <param name="max">10</param>
                 </command_interface>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>
@@ -30,9 +30,9 @@
             <joint name="${prefix}_Slide_2">
                 <!-- <param name="mimic">${prefix}_Slide_1</param>
                 <param name="multiplier">1</param> -->
-                <command_interface name="position">
+                <command_interface name="effort">
-                    <param name="min">-1</param>
+                    <param name="min">-10</param>
-                    <param name="max"> 1</param>
+                    <param name="max">10</param>
                 </command_interface>
                 <state_interface name="position"/>
                 <state_interface name="velocity"/>

robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp

@@ -13,9 +13,13 @@ class MoveToPose : public BtAction<MoveToPoseAction>
     public:
         MoveToPose(const std::string & name, const BT::NodeConfiguration & config) 
         : BtAction<MoveToPoseAction>(name, config) {
-            target_pose_.position.x = 0.1;
+            target_pose_.position.x = 0.2;
-            target_pose_.position.y = 0.1;
+            target_pose_.position.y = 0.2;
-            target_pose_.position.z = 0.6;
+            target_pose_.position.z = 0.7;
+            //target_pose_.orientation.x = 0;
+            //target_pose_.orientation.y = 0.9764;
+            //target_pose_.orientation.z = 0;
+            //target_pose_.orientation.w = 0.2160;
         }
 
         MoveToPoseAction::Goal populate_goal() override

robossembler_interfaces/CMakeLists.txt

@@ -26,7 +26,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/PropertyValuePair.msg"
     "msg/ActionFeedbackStatusConstants.msg"
     "msg/ActionResultStatusConstants.msg"
-    DEPENDENCIES geometry_msgs
+    "srv/GripperCommand.srv"
+    DEPENDENCIES geometry_msgs std_msgs
 )
 
 if(BUILD_TESTING)

robossembler_interfaces/srv/GripperCommand.srv

@@ -0,0 +1,3 @@
+float64 value
+---
+string success

robossembler_servers/CMakeLists.txt

@@ -27,6 +27,7 @@ find_package(moveit_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(robossembler_interfaces REQUIRED)
+find_package(rmw REQUIRED)
 
 set(deps
   rclcpp
@@ -52,6 +53,15 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+
+add_library(gripper_cmd_srv_lib SHARED src/gripper_cmd_srv.cpp)
+target_compile_definitions(gripper_cmd_srv_lib PRIVATE "GRIPPER_CMD_BUILDING_DLL")
+ament_target_dependencies(gripper_cmd_srv_lib ${deps})
+
+rclcpp_components_register_node(gripper_cmd_srv_lib
+  PLUGIN "GripperCmd"
+  EXECUTABLE gripper_cmd_srv)
+
 add_executable(move_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
 # target_include_directories(move_to_joint_states_action_server PRIVATE include)
 ament_target_dependencies(move_to_joint_states_action_server ${deps})
@@ -60,11 +70,16 @@ add_executable(move_topose_action_server src/move_topose_action_server.cpp)
 # target_include_directories(move_topose_action_server PRIVATE include)
 ament_target_dependencies(move_topose_action_server ${deps})
 
+
 install(
   TARGETS 
   move_topose_action_server
   move_to_joint_states_action_server
-  DESTINATION lib/${PROJECT_NAME}
+  gripper_cmd_srv_lib
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
+
 ament_package()

robossembler_servers/src/gripper_cmd_srv.cpp

@@ -0,0 +1,46 @@
+#include <memory>
+#include <string>
+#include <vector>
+#include <cinttypes>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "std_msgs/msg/float64_multi_array.hpp"
+#include "robossembler_interfaces/srv/gripper_command.hpp"
+
+using GripperCmdSrv = robossembler_interfaces::srv::GripperCommand;
+
+class GripperCmd : public rclcpp::Node
+{
+  public:
+    explicit GripperCmd(const rclcpp::NodeOptions & options)
+    : Node("gripper_cmd", options)
+    {
+        setvbuf(stdout, NULL, _IONBF, BUFSIZ);
+        auto handle_gripper_cmd = 
+        [this](const std::shared_ptr<rmw_request_id_t> request_header,
+        const std::shared_ptr<GripperCmdSrv::Request> request,
+        std::shared_ptr<GripperCmdSrv::Response> response) -> void
+        {
+            RCLCPP_INFO(this->get_logger(), "Service got request value [%f]", request->value);
+            (void)request_header;
+            RCLCPP_INFO(this->get_logger(), "request header");
+            //commands = request->value;
+            std_msgs::msg::Float64MultiArray msg;
+            RCLCPP_INFO(this->get_logger(), "init msg");
+            msg.data[0] = request->value;
+            RCLCPP_INFO(this->get_logger(), "write to msg");
+            RCLCPP_INFO(this->get_logger(), "Publishing message");
+            pub_->publish(msg);
+            response->success = "SUCCESS";
+
+        };
+        srv_ = create_service<GripperCmdSrv>("/gripper_cmd", handle_gripper_cmd);
+        pub_ = create_publisher<std_msgs::msg::Float64MultiArray>("/rasmt_hand_controller/commands", 10);
+    }
+  private:
+    rclcpp::Service<GripperCmdSrv>::SharedPtr srv_;
+    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr pub_;
+}; // class GripperCmd
+
+RCLCPP_COMPONENTS_REGISTER_NODE(GripperCmd)

robossembler_servers/src/move_topose_action_server.cpp

@@ -18,6 +18,7 @@
 // moveit libs
 #include "moveit/move_group_interface/move_group_interface.h"
 #include "moveit/planning_interface/planning_interface.h"
+#include "moveit/robot_model_loader/robot_model_loader.h"
 
 /*
 #include <tf2/LinearMath/Quaternion.h>
@@ -109,9 +110,12 @@ private:
         RCLCPP_INFO(this->get_logger(), "Executing action goal");
         const auto goal = goal_handle->get_goal();
         auto result = std::make_shared<MoveitSendPose::Result>();
-
+        
         moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
 
+        std::copy(move_group_interface.getJointModelGroupNames().begin(), move_group_interface.getJointModelGroupNames().end(),
+            std::ostream_iterator<std::string>(std::cout, ", "));
+        move_group_interface.setStartState(*move_group_interface.getCurrentState());
         move_group_interface.setPoseTarget(goal->target_pose);
         move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
         move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
@@ -121,7 +125,7 @@ private:
         if(success)
         {
             RCLCPP_INFO(this->get_logger(), "Planning success");
-            move_group_interface.execute(plan);
+            //move_group_interface.execute(plan);
             move_group_interface.move();
         }else{
             RCLCPP_WARN(this->get_logger(), "Failed to generate plan");