Merge pull request #140 from elephantrobotics/myhand_force_gripper

Myhand force gripper

README.md

@@ -224,10 +224,22 @@ Please adhere to this project's [code of conduct](CODE_OF_CONDUCT.md).
 
 ![320 m5 2022 gripper](./demo_img/320m5_2022/320m5_gripper_2022.png)
 
+[mycobot 320 m5 2022 force control gripper](./mycobot_description/urdf/mycobot_320_m5_2022/pro_320_m5_2022_force_gripper.urdf)
+
+![320 m5 2022 force control gripper](./demo_img/320m5_2022/320m5_force_gripper_2022.png)
+
 [mycobot 320 pi 2022](./mycobot_description/urdf/mycobot_320_pi_2022/new_mycobot_pro_320_pi_2022.urdf)
 
 ![320 pi 2022](./demo_img/320pi_2022/320pi_2022.png)
 
+[mycobot 320 pi 2022 gripper](./mycobot_description/urdf/mycobot_320_pi_2022/new_mycobot_pro_320_pi_2022_gripper.urdf)
+
+![320 m5 2022 gripper](./demo_img/320m5_2022/320m5_gripper_2022.png)
+
+[mycobot 320 pi 2022 force control gripper](./mycobot_description/urdf/mycobot_320_pi_2022/pro_320_pi_2022_force_gripper.urdf)
+
+![320 m5 2022 force control gripper](./demo_img/320m5_2022/320m5_force_gripper_2022.png)
+
 [ultraArm P340](./mycobot_description/urdf/ultraArm_p340/ultraArm_p340.urdf)
 
 ![p340](./demo_img/ultraArm_p340/ultraArmp340.png)

mycobot_320/new_mycobot_320/CMakeLists.txt

@@ -71,6 +71,7 @@ catkin_install_python(PROGRAMS
   scripts/mycobot_320_listen_real.py
   scripts/mycobot_320_listen_real_of_topic.py
   scripts/mycobot_320_simple_gui.py
+  scripts/mycobot_320_force_gripper_slider.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 
 )

mycobot_320/new_mycobot_320/config/new_mycobot_320_gripper.rviz

@@ -13,7 +13,7 @@ Panels:
         - /TF1/Frames1
         - /TF1/Tree1
       Splitter Ratio: 0.5
-    Tree Height: 657
+    Tree Height: 609
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -249,17 +249,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3197958171367645
+      Pitch: 0.42979562282562256
       Target Frame: <Fixed Frame>
-      Yaw: 3.108586549758911
+      Yaw: 3.2435879707336426
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 954
+  Height: 906
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000031cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000031c000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005c80000031c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002ec000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005c8000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

mycobot_320/new_mycobot_320/launch/adaptive_gripper.launch

@@ -0,0 +1,17 @@
+<launch>
+	<arg name="model" default="$(find mycobot_description)/urdf/pro_adaptive_gripper/mycobot_pro_adaptive_gripper.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_description)/urdf/pro_adaptive_gripper/pro_adaptive_gripper.rviz" />
+
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
+		<param name="use_gui" value="$(arg gui)" />
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_320/new_mycobot_320/launch/force_gripper.launch

@@ -0,0 +1,17 @@
+<launch>
+    <arg name="model" default="$(find mycobot_description)/urdf/force_control_gripper/mycobot_pro_force_gripper.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_description)/urdf/pro_adaptive_gripper/pro_adaptive_gripper.rviz" />
+
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
+		<param name="use_gui" value="$(arg gui)" />
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_320/new_mycobot_320/launch/mycobot_320_force_gripper_slider.launch

@@ -0,0 +1,20 @@
+<launch>
+	<arg name="port" default="/dev/ttyACM0" />
+	<arg name="baud" default="115200" />
+
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_320_m5_2022/pro_320_m5_2022_force_gripper.urdf"/>
+	<arg name="rvizconfig" default="$(find new_mycobot_320)/config/new_mycobot_320_gripper.rviz" />
+	<arg name="gui" default="true" />
+
+	<!-- new -->
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
+
+	</node>
+
+	<!-- Show in Rviz -->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_320/new_mycobot_320/launch/mycobot_320_force_gripper_test.launch

@@ -0,0 +1,17 @@
+<launch>
+	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_320_m5_2022/pro_320_m5_2022_force_gripper.urdf"/>
+	<arg name="rvizconfig" default="$(find new_mycobot_320)/config/new_mycobot_320_gripper.rviz" />
+
+	<arg name="gui" default="true" />
+
+	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+	<!-- Combinejoin values to TF -->
+	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui">
+		<param name="use_gui" value="$(arg gui)" />
+		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+	</node>
+	<!-- Show in Rviz ，显示在Rviz-->
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
+</launch>

mycobot_320/new_mycobot_320/launch/mycobot_320_gripper_slider.launch

@@ -1,31 +1,20 @@
 <launch>
-	<arg name="port" default="/dev/ttyUSB0" />
+	<arg name="port" default="/dev/ttyACM0" />
 	<arg name="baud" default="115200" />
 
 	<arg name="model" default="$(find mycobot_description)/urdf/mycobot_320_m5_2022/new_mycobot_pro_320_m5_2022_gripper.urdf"/>
 	<arg name="rvizconfig" default="$(find new_mycobot_320)/config/new_mycobot_320_gripper.rviz" />
 	<arg name="gui" default="true" />
 
-    <!-- <include file="$(find mycobot_280)/launch/slider_control.launch">
-		<arg name="model" value="$(arg model)" />
-		<arg name="rvizconfig" value="$(arg rvizconfig)" />
-		<arg name="gui" value="$(arg gui)" />
-    </include> -->
-
 	<!-- new -->
 	<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
 
 	<!-- Combinejoin values to TF -->
 	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
 	<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"> 
-		<!-- <param name="use_gui" value="$(arg gui)" />	 -->
-		<!-- <rosparam param="source_list" subst_value="true">["joint_states"]</rosparam> -->
+
 	</node>
-	<!-- Open control script -->
-	<!-- <node name="control_slider" pkg="mycobot_280" type="slider_control.py"> 
-		<param name="port" type="string" value="$(arg port)" />
-		<param name="baud" type="int" value="$(arg baud)" />
-	</node> -->
+
 	<!-- Show in Rviz -->
 	<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
 </launch>

mycobot_320/new_mycobot_320/launch/three_gripper.launch

@@ -0,0 +1,11 @@
+<launch>
+    <arg name="model" default="$(find mycobot_description)/urdf/three_gripper/mycobot_pro_three_gripper.urdf"/>
+	<arg name="rvizconfig" default="$(find mycobot_description)/urdf/three_gripper/three_gripper.rviz" />
+
+    <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
+
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+    <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="static_transform_publisher" args="0 0 0 0 0 0 world gripper_base"/> -->
+    <!-- 使用 RViz 显示 -->
+    <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>
+</launch>

mycobot_320/new_mycobot_320/scripts/mycobot_320_force_gripper_slider.py

@@ -0,0 +1,73 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+"""[summary]
+This file obtains the joint angle of the manipulator in ROS,
+and then sends it directly to the real manipulator using `pymycobot` API.
+This file is [slider_control.launch] related script.
+Passable parameters:
+    port: serial prot string. Defaults is '/dev/ttyUSB0'
+    baud: serial prot baudrate. Defaults is 115200.
+"""
+
+import rospy
+import time
+from sensor_msgs.msg import JointState
+import math
+import pymycobot
+from packaging import version
+
+# min low version require
+MIN_REQUIRE_VERSION = '3.6.4'
+
+current_verison = pymycobot.__version__
+print('current pymycobot library version: {}'.format(current_verison))
+if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
+    raise RuntimeError('{}The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
+else:
+    print('pymycobot library version meets the requirements!')
+    from pymycobot.mycobot320 import MyCobot320
+
+
+mc = None
+gripper_value = []
+
+def callback(data):
+    global mc
+    # rospy.loginfo(rospy.get_caller_id() + "%s", data.position)
+    data_list = []
+    for index, value in enumerate(data.position):
+        data_list.append(round(value, 3))
+
+    data_list = data_list[:7]
+    angles_list = [round(math.degrees(radian), 2) for radian in data_list[:6]]
+    print("angles:%s"%angles_list)
+    mc.send_angles(angles_list, 25)
+    gripper_value = int(data_list[6]* 100)
+    print("gripper_value:%s"%gripper_value)
+
+    mc.set_pro_gripper_angle(14, gripper_value)
+
+def listener():
+    global mc
+    global gripper_value
+
+    rospy.init_node("control_slider", anonymous=True)
+
+
+    port = rospy.get_param("~port", "/dev/ttyACM0")
+    baud = rospy.get_param("~baud", 115200)
+    print(port, baud)
+    mc = MyCobot320(port, baud)
+    time.sleep(0.05)
+    mc.set_fresh_mode(1)
+    time.sleep(0.05)
+
+    rospy.Subscriber("joint_states", JointState, callback)
+
+    # spin() simply keeps python from exiting until this node is stopped
+    print("spin ...")
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    listener()

mycobot_320/new_mycobot_320/scripts/mycobot_320_gripper_slider.py

@@ -11,6 +11,7 @@
 
 import rospy
 import time
+import math
 from sensor_msgs.msg import JointState
 
 from pymycobot.mycobot import MyCobot
@@ -27,25 +28,28 @@ def callback(data):
         data_list.append(round(value, 3))
 
     data_list = data_list[:7]
-    print("radians:%s"%data_list[:6])
-    mc.send_radians(data_list[:6], 80)
+    angles_list = [round(math.degrees(radian), 2) for radian in data_list[:6]]
+    print("angles:%s"%angles_list)
+    mc.send_angles(angles_list, 25)
     gripper_value = int(abs(-0.7-data_list[6])* 117)
     print("gripper_value:%s"%gripper_value)
-    # mc.set_gripper_mode(0)
-    # time.sleep(1)
-    mc.set_gripper_value(gripper_value, 100)
+
+    mc.set_gripper_value(gripper_value, 80)
 
 def listener():
     global mc
     global gripper_value
 
     rospy.init_node("control_slider", anonymous=True)
 
-    rospy.Subscriber("joint_states", JointState, callback)
-    port = rospy.get_param("~port", "/dev/ttyUSB0")
+    port = rospy.get_param("~port", "/dev/ttyACM0")
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyCobot(port, baud)
+    time.sleep(0.05)
+    mc.set_fresh_mode(1)
+    time.sleep(0.05)
+    rospy.Subscriber("joint_states", JointState, callback)
 
     # spin() simply keeps python from exiting until this node is stopped
     print("spin ...")

mycobot_320/new_mycobot_320/scripts/mycobot_320_slider.py

@@ -34,14 +34,14 @@ def listener():
 
     rospy.init_node("control_slider", anonymous=True)
 
-    rospy.Subscriber("joint_states", JointState, callback)
-    port = rospy.get_param("~port", "/dev/ttyUSB0")
+    port = rospy.get_param("~port", "/dev/ttyACM0")
     baud = rospy.get_param("~baud", 115200)
     print(port, baud)
     mc = MyCobot(port, baud)
     time.sleep(0.05)
     mc.set_fresh_mode(1)
     time.sleep(0.05)
+    rospy.Subscriber("joint_states", JointState, callback)
     # spin() simply keeps python from exiting until this node is stopped
     print("spin ...")
     rospy.spin()

mycobot_320/new_mycobot_320_force_gripper_moveit/.setup_assistant

@@ -0,0 +1,11 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: mycobot_description
+    relative_path: urdf/mycobot_320_m5_2022/pro_320_m5_2022_moveit_force_gripper.urdf
+    xacro_args: ""
+  SRDF:
+    relative_path: config/firefighter.srdf
+  CONFIG:
+    author_name: wangweijian
+    author_email: [email protected]
+    generated_timestamp: 1732845154

mycobot_320/new_mycobot_320_force_gripper_moveit/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.1.3)
+project(new_mycobot_320_force_gripper_moveit)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  PATTERN "setup_assistant.launch" EXCLUDE)
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

mycobot_320/new_mycobot_320_force_gripper_moveit/config/cartesian_limits.yaml

@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57

mycobot_320/new_mycobot_320_force_gripper_moveit/config/chomp_planning.yaml

@@ -0,0 +1,18 @@
+planning_time_limit: 10.0
+max_iterations: 200
+max_iterations_after_collision_free: 5
+smoothness_cost_weight: 0.1
+obstacle_cost_weight: 1.0
+learning_rate: 0.01
+smoothness_cost_velocity: 0.0
+smoothness_cost_acceleration: 1.0
+smoothness_cost_jerk: 0.0
+ridge_factor: 0.0
+use_pseudo_inverse: false
+pseudo_inverse_ridge_factor: 1e-4
+joint_update_limit: 0.1
+collision_clearance: 0.2
+collision_threshold: 0.07
+use_stochastic_descent: true
+enable_failure_recovery: false
+max_recovery_attempts: 5

mycobot_320/new_mycobot_320_force_gripper_moveit/config/fake_controllers.yaml

@@ -0,0 +1,19 @@
+controller_list:
+  - name: fake_arm_group_controller
+    type: $(arg fake_execution_type)
+    joints:
+      - joint2_to_joint1
+      - joint3_to_joint2
+      - joint4_to_joint3
+      - joint5_to_joint4
+      - joint6_to_joint5
+      - joint6output_to_joint6
+  - name: fake_gripper_group_controller
+    type: $(arg fake_execution_type)
+    joints:
+      - gripper_controller
+initial:  # Define initial robot poses per group
+  - group: arm_group
+    pose: init_pose
+  - group: gripper_group
+    pose: init_gripper