b-it-bots · vamsikalagaturu · Feb 17, 2023 · Feb 22, 2023 · Feb 22, 2023 · Feb 22, 2023
diff --git a/.isort.cfg b/.isort.cfg
@@ -1,5 +1,5 @@
 [settings]
-known_third_party = PIL,actionlib,actionlib_msgs,arm_navigation_msgs,at_work_robot_example_ros,atwork_ros_msgs,brics_actuator,catkin_pkg,diagnostic_msgs,dynamic_reconfigure,geometry_msgs,mas_perception_msgs,mcr_common_converters_ros,mcr_manipulation_measurers_ros,mcr_manipulation_msgs,mcr_manipulation_pose_selector_ros,mcr_perception_msgs,mcr_perception_states,mcr_pose_generation_ros,mcr_states,mercury_planner,mir_actions,mir_controller_msgs,mir_knowledge,mir_knowledge_ros,mir_navigation_msgs,mir_planning_msgs,mir_planning_visualisation,mir_pregrasp_planning,mir_pregrasp_planning_ros,mir_states,mir_states_common,mir_world_generation,move_base_msgs,moveit_commander,moveit_msgs,nav_msgs,numpy,pandas,param_server_utils,roslib,rospkg,rosplan_dispatch_msgs,rosplan_knowledge_msgs,rospy,rostest,rosunit,sensor_msgs,shape_msgs,simple_script_server,smach,smach_ros,std_msgs,std_srvs,tf,visualization_msgs,yaml,zmq
+known_third_party = PIL,actionlib,actionlib_msgs,arm_navigation_msgs,at_work_robot_example_ros,atwork_ros_msgs,brics_actuator,catkin_pkg,diagnostic_msgs,dynamic_reconfigure,geometry_msgs,mas_perception_msgs,mcr_common_converters_ros,mcr_manipulation_measurers_ros,mcr_manipulation_msgs,mir_manipulation_msgs,mcr_manipulation_pose_selector_ros,mcr_perception_msgs,mcr_perception_states,mcr_pose_generation_ros,mcr_states,mercury_planner,mir_actions,mir_controller_msgs,mir_knowledge,mir_knowledge_ros,mir_navigation_msgs,mir_planning_msgs,mir_planning_visualisation,mir_pregrasp_planning,mir_pregrasp_planning_ros,mir_states,mir_states_common,mir_world_generation,move_base_msgs,moveit_commander,moveit_msgs,nav_msgs,numpy,pandas,param_server_utils,roslib,rospkg,rosplan_dispatch_msgs,rosplan_knowledge_msgs,rospy,rostest,rosunit,sensor_msgs,shape_msgs,simple_script_server,smach,smach_ros,std_msgs,std_srvs,tf,visualization_msgs,yaml,zmq
 multi_line_output=3
 include_trailing_comma=True
 force_grid_wrap=0

diff --git a/docs/source/mir_planning/planning_core.rst b/docs/source/mir_planning/planning_core.rst
@@ -29,7 +29,7 @@ To test with mockup action servers without a robot
 
     roscore
     roslaunch mir_planning_core task_planning_components.launch
-    rosrun mir_task_executor task_executor_mockup
+    rosrun mir_planner_executor planner_executor_mockup
     roslaunch mir_task_planning upload_problem.launch
     roslaunch mir_planning_core task_planning_sm.launch
 

diff --git a/mir_command_tools/mir_teleop/CMakeLists.txt b/mir_command_tools/mir_teleop/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(catkin REQUIRED
     sensor_msgs
     std_srvs
     mcr_manipulation_msgs
+    mir_manipulation_msgs
     roslint
 )
 
@@ -33,6 +34,7 @@ catkin_package(
     sensor_msgs
     std_srvs
     mcr_manipulation_msgs
+    mir_manipulation_msgs
 )
 
 include_directories(

diff --git a/mir_command_tools/mir_teleop/package.xml b/mir_command_tools/mir_teleop/package.xml
@@ -22,6 +22,7 @@
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>mcr_manipulation_msgs</build_depend>
+  <build_depend>mir_manipulation_msgs</build_depend>
   <build_depend>roslint</build_depend>
 
   <run_depend>brics_actuator</run_depend>
@@ -34,6 +35,7 @@
   <run_depend>sensor_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>mcr_manipulation_msgs</run_depend>
+  <run_depend>mir_manipulation_msgs</run_depend>
 
   <test_depend>roslaunch</test_depend>
 

diff --git a/mir_command_tools/mir_teleop/ros/include/mir_teleop/teleop_joypad.h b/mir_command_tools/mir_teleop/ros/include/mir_teleop/teleop_joypad.h
@@ -17,7 +17,7 @@
 #include <dynamic_reconfigure/server.h>
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/TwistStamped.h>
-#include <mcr_manipulation_msgs/GripperCommand.h>
+#include <mir_manipulation_msgs/GripperCommand.h>
 #include <moveit_msgs/JointLimits.h>
 #include <ros/ros.h>
 #include <sensor_msgs/JointState.h>

diff --git a/mir_command_tools/mir_teleop/ros/src/teleop_joypad.cpp b/mir_command_tools/mir_teleop/ros/src/teleop_joypad.cpp
@@ -53,7 +53,7 @@ TeleOpJoypad::TeleOpJoypad(ros::NodeHandle &nh)
   pub_arm_cart_vel_ = nh_->advertise<geometry_msgs::TwistStamped>(
       "/arm_1/arm_controller/cartesian_velocity_command", 1);
   pub_gripper_command_ =
-      nh_->advertise<mcr_manipulation_msgs::GripperCommand>("gripper_command", 1);
+      nh_->advertise<mir_manipulation_msgs::GripperCommand>("gripper_command", 1);
 
   srv_base_motors_on_ = nh_->serviceClient<std_srvs::Empty>("/base/switchOnMotors");
   srv_base_motors_off_ = nh_->serviceClient<std_srvs::Empty>("/base/switchOffMotors");
@@ -203,7 +203,7 @@ bool TeleOpJoypad::getArmParameter()
 
 bool TeleOpJoypad::moveGripper(int gripper_command)
 {
-  mcr_manipulation_msgs::GripperCommand command_msgs;
+  mir_manipulation_msgs::GripperCommand command_msgs;
 
   command_msgs.command = gripper_command;
 
@@ -337,10 +337,10 @@ void TeleOpJoypad::cbJoypad(const sensor_msgs::Joy::ConstPtr &command)
       button_gripper_active_ = !button_gripper_active_;
       if (button_gripper_active_) {
         ROS_INFO("open gripper");
-        this->moveGripper(mcr_manipulation_msgs::GripperCommand::OPEN);
+        this->moveGripper(mir_manipulation_msgs::GripperCommand::OPEN);
       } else {
         ROS_INFO("close gripper");
-        this->moveGripper(mcr_manipulation_msgs::GripperCommand::CLOSE);
+        this->moveGripper(mir_manipulation_msgs::GripperCommand::CLOSE);
       }
     }
 

diff --git a/mir_common/mir_description/meshes/misc/e_stop_mount.STL b/mir_common/mir_description/meshes/misc/e_stop_mount.STL
diff --git a/mir_common/mir_description/meshes/sensor_mounts/intel_camera_mount.STL b/mir_common/mir_description/meshes/sensor_mounts/intel_camera_mount.STL
diff --git a/mir_common/mir_description/meshes/sensor_mounts/intel_camera_mount_back.STL b/mir_common/mir_description/meshes/sensor_mounts/intel_camera_mount_back.STL
diff --git a/mir_common/mir_description/meshes/sensor_mounts/side_camera_mount.STL b/mir_common/mir_description/meshes/sensor_mounts/side_camera_mount.STL
diff --git a/mir_common/mir_description/urdf/grippers/dynamixel/v2/dynamixel_gripper.urdf.xacro b/mir_common/mir_description/urdf/grippers/dynamixel/v2/dynamixel_gripper.urdf.xacro
@@ -32,20 +32,23 @@
     <!-- motor mount -->
     <link name="${name}_motor_mount_link">
       <inertial>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <origin xyz="-0.023 -0.046 0.024" rpy="0 0 0"/>
         <mass value="0.038"/>
         <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01" />
       </inertial>
 
       <visual>
-        <origin xyz="0 0 0.0" rpy="0 ${M_PI/2} 0" />
+        <origin xyz="-0.023 -0.046 0.024" rpy="0 ${M_PI/2} 0" />
         <geometry>
           <mesh filename="package://mir_description/meshes/grippers/dynamixel/v3/dynamixel_intel_camera_combined.stl" scale="0.001 0.001 0.001" />
         </geometry>
+        <material name="">
+          <color rgba="0.85098 0.85098 0.85098 1"/>
+        </material>
       </visual>
 
       <collision>
-        <origin xyz="0 0 -0.0" rpy="0 ${M_PI/2} 0" />
+        <origin xyz="-0.023 -0.046 0.024" rpy="0 ${M_PI/2} 0" />
         <geometry>
           <mesh filename="package://mir_description/meshes/grippers/dynamixel/v2/dynamixel_motor_mount_convex_hull.dae" scale="0.001 0.001 0.001" />
         </geometry>

diff --git a/mir_manipulation/mir_gripper_controller/CMakeLists.txt b/mir_manipulation/mir_gripper_controller/CMakeLists.txt
@@ -4,15 +4,15 @@ project(mir_gripper_controller)
 find_package(catkin REQUIRED
   COMPONENTS
     actionlib
-    mcr_manipulation_msgs
+    mir_manipulation_msgs
     roscpp
     roslint
     std_msgs
 )
 
 catkin_package(
   CATKIN_DEPENDS
-    mcr_manipulation_msgs
+    mir_manipulation_msgs
     rospy
 )
 

diff --git a/mir_manipulation/mir_gripper_controller/README.md b/mir_manipulation/mir_gripper_controller/README.md
@@ -14,11 +14,35 @@ More details on the controller and corresponding code can be found [here](https:
 1. Make sure to uplaod the latest [code](https://github.com/b-it-bots/youbot_dynamixel_gripper_controller/tree/master/openrb_150_controller) on to the controller.
 5. Run `roscore` command.
 5. Run `dynamixel_gripper_controller_ros` in the `ros/scripts` directory.
-6. To control the gripper publish to the `/arm_1/gripper_command` topic, e.g. `rostopic pub /arm_1/gripper_command std_msgs/String "command: 0.0"`
+6. To control the gripper publish to the `/arm_1/gripper_command` topic, e.g.:
+    ```bash
+    rostopic pub /arm_1/gripper_command mir_manipulation_msgs/GripperCommand "command: 0.0"
+    ```
 7. To see the state of the gripper subscribe to the `/arm_1/gripper_feedback`, e.g. `rostopic echo /arm_1/gripper_feedback`.
 
 ## Usage
 * One can publish `0.0 (open)` or `1.0 (close)` command to the gripper.
 * For more specific position control, one can publish a value between `0.0 and 1.0`.
 * To further open the gripper, one can publish a value between `-1.5 and 0.0`.
 * One can subscribe the state of the gripper, the format consists of the following fields: `state` (`GRIPPER_CLOSE`, `OBJECT_GRASPED`, `OBJECT_SLIPPED`, `GRIPPER_OPEN`, `GRIPPER_INTER`), `parsing_error` (0 - false, 1 - true), `last_command` (0, 1).
+
+## Troubleshooting
+
+### Board at /dev/youbout/gripper not found
+* Check if the board is connected to the computer.
+* Check if the board is connected to the correct USB port.
+* Check the `idProduct` of the Open-RB board is properly set in /etc/udev/rules.d/80-youboot-brsu-2-devices.rules
+* To get the `idProduct` of the Open-RB board:
+    - run `dmesg | grep OpenRB`, you will see something like this:
+        ```bash
+            [    6.449181] usb 1-2.3: Product: OpenRB-150
+        ```
+    - run `dmesg | grep "usb 1-2.3"`: in the generated output, find the line that looks like:
+        ```bash
+             usb 1-2.3: New USB device found, idVendor=2f5d, idProduct=2202, bcdDevice= 1.00
+        ```
+    - From this, get the `idProduct` and update it in the file `/etc/udev/rules.d/80-youboot-brsu-2-devices.rules`
+    - Then run `sudo udevadm control --reload`
+    - Now, unplug and plug the board again. It should be detected now.
+    - To test, run the `serial_interface.py` file in the `ros/src` directory. It should close and open the gripper.
+
diff --git a/mir_manipulation/mir_gripper_controller/package.xml b/mir_manipulation/mir_gripper_controller/package.xml
@@ -20,10 +20,10 @@
   <build_depend>roslint</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>mcr_manipulation_msgs</build_depend>
+  <build_depend>mir_manipulation_msgs</build_depend>
 
   <run_depend>dynamixel_controllers</run_depend>
-  <run_depend>mcr_manipulation_msgs</run_depend>
+  <run_depend>mir_manipulation_msgs</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>brics_actuator</run_depend>

diff --git a/mir_manipulation/mir_gripper_controller/ros/scripts/dynamixel_gripper_controller_ros b/mir_manipulation/mir_gripper_controller/ros/scripts/dynamixel_gripper_controller_ros
@@ -3,7 +3,7 @@
 import rospy
 from std_msgs.msg import String
 from sensor_msgs.msg import JointState
-from mcr_manipulation_msgs.msg import GripperCommand
+from mir_manipulation_msgs.msg import GripperCommand
 import serial_interface
 import math
 import json
@@ -92,8 +92,8 @@ class GripperController:
                 self.feedback_publisher.publish(json.dumps(msg))
 
                 # convert gripper positions from degrees to radians
-                gripper_right_motor_position = math.radians(msg['right_gripper_pos'])
-                gripper_left_motor_position = math.radians(msg['left_gripper_pos'])
+                gripper_right_motor_position = 0.0 # math.radians(msg['right_gripper_pos'])
+                gripper_left_motor_position = 0.0 # math.radians(msg['left_gripper_pos'])
 
                 # Publish joint states
                 joint_state = JointState()

diff --git a/mir_manipulation/mir_gripper_controller/ros/scripts/list_ports.py b/mir_manipulation/mir_gripper_controller/ros/scripts/list_ports.py
@@ -0,0 +1,5 @@
+import serial.tools.list_ports
+ports = serial.tools.list_ports.comports()
+
+for port, desc, hwid in sorted(ports):
+        print("{}: {} [{}]".format(port, desc, hwid))
diff --git a/mir_manipulation/mir_gripper_controller/ros/scripts/serial_interface.py b/mir_manipulation/mir_gripper_controller/ros/scripts/serial_interface.py
@@ -58,12 +58,15 @@ def receive(self):
 
         msg_str = msg_str.decode().strip()
 
-
         if len(msg_str) < 5 or not '{' in msg_str:
             return None
 
         if '}{' not in msg_str:
-            return [json.loads(msg_str)]
+            try:
+                return [json.loads(msg_str)]
+            except Exception as e:
+                rospy.logerr(e)
+                return None
 
         msgs = msg_str.split("}{")
 
@@ -79,3 +82,18 @@ def receive(self):
             msgs_json.append(json.loads(msg))
 
         return msgs_json
+
+if __name__ == '__main__':
+    serial_interface = SerialInterface(9600, 0.1, '239A')
+    serial_interface.open_port()
+    serial_interface.send({'command': 1})
+    rospy.sleep(1)
+    serial_interface.send({'command': 0.5})
+    rospy.sleep(1)
+    serial_interface.send({'command': 0.0})
+    rospy.sleep(1)
+    serial_interface.send({'command': 0.2})
+    rospy.sleep(1)
+    serial_interface.send({'command': 0.8})
+    rospy.sleep(1)
+    serial_interface.send({'command': 0.0})
diff --git a/mir_manipulation/mir_moveit_youbot_brsu_2/config/joint_limits.yaml b/mir_manipulation/mir_moveit_youbot_brsu_2/config/joint_limits.yaml
@@ -37,5 +37,5 @@ joint_limits:
     max_velocity: 6.178465545
     has_acceleration_limits: true
     max_acceleration: 1.5
-default_velocity_scaling_factor: 0.7
-default_acceleration_scaling_factor: 0.7
+default_velocity_scaling_factor: 1.0
+default_acceleration_scaling_factor: 1.0