update

CMakeLists.txt

@@ -5,7 +5,7 @@ cmake_minimum_required(VERSION 2.8.3)
 project(ur5_notebook)
 
 # Find the catkin build system, and any other packages on which we depend.
-find_package(catkin REQUIRED COMPONENTS gazebo_msgs roscpp geometry_msgs turtlesim std_srvs std_msgs message_generation)
+find_package(catkin REQUIRED COMPONENTS gazebo_msgs roscpp geometry_msgs std_srvs std_msgs message_generation)
 # Declare our catkin package.
 
 #add_message files

blocks_spawner.cpp

@@ -89,7 +89,7 @@
       /*red_inXml.open(red_box_path.c_str());*/
       red_strStream << red_inXml.rdbuf();
       red_xmlStr = red_strStream.str();
-      ROS_INFO_STREAM("urdf: \n" <<red_xmlStr);
+     // ROS_INFO_STREAM("urdf: \n" <<red_xmlStr);
       // prepare the pawn model service message
       spawn_model_req.initial_pose.position.x = 4.5;
       spawn_model_req.initial_pose.position.z = 0.2;
@@ -101,7 +101,7 @@
 
       ros::Time time_temp(0, 0);
       ros::Duration duration_temp(0, 1000000);
-      apply_wrench_req.wrench.force.x = -0.001;
+      apply_wrench_req.wrench.force.x = -10;
       apply_wrench_req.wrench.force.y = 0.0;
       apply_wrench_req.wrench.force.z = 0.0;
       apply_wrench_req.start_time = time_temp;

launch/initialize.launch

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <launch>
-  <param name="red_box_path" type="str" value="/home/haoran/ros_hw/src/ur5_notebook/urdf/red_box.urdf"/>
+  <param name="red_box_path" type="str" value="/home/lihuang/ur5_ws/src/ur5_notebook/urdf/red_box.urdf"/>
 
   <arg name="limited" default="true"/>
   <arg name="paused" default="false"/>
@@ -48,7 +48,6 @@
 
   <!-- the cylinder poses publisher node -->
   <node name="ur5_vision" pkg="ur5_notebook" type="ur5_vision.py" output="screen" />
-  <!-- the cylinder poses publisher node -->
   <node name="ur5_mp" pkg="ur5_notebook" type="ur5_mp.py" output="screen" />
   <!-- for ros control to be used with scara robot -->
 <!--   <param name="/scara_robot_left/robot_description" textfile="$(find two_scara_collaboration)/urdf/scara_robot_left.urdf" /> -->

src/ur_description/common.gazebo.xacro

@@ -6,6 +6,8 @@
       <!--robotNamespace>/</robotNamespace-->
       <!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
     </plugin>
+
+
 
 <!--
     <plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
@@ -21,7 +23,13 @@
       <chargeVoltage>16.41</chargeVoltage>
     </plugin>
 -->
+
   </gazebo>
+
+
+
+
+
   <!-- camera_link -->
   <gazebo reference="camera_link">
     <mu1>0.2</mu1>

src/ur_description/ur5.urdf.xacro

@@ -318,6 +318,46 @@
       </collision>
     </link>
 
+    <link name="vacuum_gripper">
+      <gravity>0</gravity>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <cylinder radius="0.02" length="0.02"/>
+        </geometry>
+        <material name="transparent">
+          <color rgba="0 0 0 0"/>
+        </material>
+      </visual>
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
+        <mass value="0.0001"/>
+        <inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
+      </inertial>
+    </link>
+
+    <joint name="gripper_joint" type="revolute">
+      <axis xyz="1 0 0" />
+      <parent link="ee_link" />
+      <child link="vacuum_gripper" />
+      <origin rpy="0 1.5708 0" xyz="0.01 0 0" />
+      <limit effort="50" velocity="10" lower="0" upper="0" />
+      <dynamics damping="0.0" friction="0.2"/>
+    </joint>
+
+
+
+  <gazebo>
+      <plugin name="gazebo_ros_vacuum_gripper" filename="libgazebo_ros_vacuum_gripper.so">
+        <robotNamespace>/ur5/vacuum_gripper</robotNamespace>
+        <bodyName>vacuum_gripper</bodyName>
+        <topicName>grasping</topicName>
+	<maxForce>50</maxForce>
+        <maxDistance>1.5</maxDistance>
+        <minDistance>1.5</minDistance>
+      </plugin>
+  </gazebo>
+
   <joint name="camera_joint" type="fixed">
     <axis xyz="0 1 0" />
     <origin xyz="-0.02 0.0 0.055" rpy="0 0 0"/>

ur5_gripper.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+
+
+
+import rospy, sys, numpy as np
+import geometry_msgs.msg
+import moveit_msgs.msg
+from ur5_notebook.msg import Tracker
+from std_msgs.msg import Header
+from std_msgs.msg import Bool
+from std_srvs.srv import Empty
+
+def gripper_status(msg):
+    print('gripper status = {}'.format(msg.data))
+
+def gripper_on():
+    # Wait till the srv is available
+    rospy.wait_for_service('/ur5/vacuum_gripper/on')
+    try:
+        # Create a handle for the calling the srv
+        turn_on = rospy.ServiceProxy('/ur5/vacuum_gripper/on', Empty)
+        # Use this handle just like a normal function and call it
+        resp = turn_on()
+        return resp
+    except rospy.ServiceException, e:
+        print "Service call failed: %s" % e
+
+def gripper_off():
+    rospy.wait_for_service('/ur5/vacuum_gripper/off')
+    try:
+        turn_off = rospy.ServiceProxy('/ur5/vacuum_gripper/off', Empty)
+        resp = turn_off()
+        return resp
+    except rospy.ServiceException, e:
+        print "Service call failed: %s" % e
+
+def trigger(msg):
+    gripper_trigger = msg.flag2
+    if gripper_trigger:
+        gripper_on()
+    else:
+        gripper_off()
+
+rospy.init_node("ur5_gripper", anonymous=False)
+
+gripper_status_sub = rospy.Subscriber('/ur5/vacuum_gripper/grasping', Bool, gripper_status, queue_size=1)
+
+cxy_sub = rospy.Subscriber('cxy', Tracker, trigger, queue_size=1)
+
+rospy.spin()
+

ur5_mp.py

@@ -29,18 +29,21 @@
 import moveit_msgs.msg
 from sensor_msgs.msg import Image
 from ur5_notebook.msg import Tracker
-
-
 from std_msgs.msg import Header
 from trajectory_msgs.msg import JointTrajectory
 from trajectory_msgs.msg import JointTrajectoryPoint
 
 
+tracker = Tracker()
+
+
 
 class ur5_mp:
     def __init__(self):
         rospy.init_node("ur5_mp", anonymous=False)
         self.cxy_sub = rospy.Subscriber('cxy', Tracker, self.tracking_callback, queue_size=1)
+        self.cxy_pub = rospy.Publisher('cxy', Tracker, queue_size=1)
+        self.phase = 1
 
         self.track_flag = False
         self.default_pose_flag = True
@@ -75,8 +78,8 @@ def __init__(self):
         self.arm.set_goal_position_tolerance(0.01)
         self.arm.set_goal_orientation_tolerance(0.1)
         self.arm.set_planning_time(0.1)
-        self.arm.set_max_acceleration_scaling_factor(1)
-        self.arm.set_max_velocity_scaling_factor(1)
+        self.arm.set_max_acceleration_scaling_factor(.2)
+        self.arm.set_max_velocity_scaling_factor(.4)
 
         # Get the current pose so we can add it as a waypoint
         start_pose = self.arm.get_current_pose(self.end_effector_link).pose
@@ -101,9 +104,9 @@ def __init__(self):
         wpose.position.z = 0.4005
 
         wpose.orientation.x = 0.4811
-        wpose.orientation.y = 0.4994
-        wpose.orientation.z = -0.5121
-        wpose.orientation.w = 0.5069
+        wpose.orientation.y = 0.5070
+        wpose.orientation.z = -0.5047
+        wpose.orientation.w = 0.5000
 
         self.waypoints.append(deepcopy(wpose))
 
@@ -158,13 +161,16 @@ def cleanup(self):
         moveit_commander.os._exit(0)
 
     def tracking_callback(self, msg):
+
         self.track_flag = msg.flag1
         self.cx = msg.x
         self.cy = msg.y
         self.error_x = msg.error_x
         self.error_y = msg.error_y
         if len(self.pointx)>9:
             self.track_flag = True
+        if
+
         if (self.track_flag and -0.4 < self.waypoints[0].position.x and self.waypoints[0].position.x < 0.6):
             self.execute()
             self.default_pose_flag = False
@@ -179,6 +185,7 @@ def tracking_callback(self, msg):
     def execute(self):
         if self.track_flag:
 
+
             # Get the current pose so we can add it as a waypoint
             start_pose = self.arm.get_current_pose(self.end_effector_link).pose
 
@@ -189,52 +196,78 @@ def execute(self):
             # Append the pose to the waypoints list
             wpose = deepcopy(start_pose)
 
+            # wpose.position.x = -0.5215
+            # wpose.position.y = 0.2014
+            # wpose.position.z = 0.4102
+
+
             if len(self.pointx)>8:
                 if len(self.pointx)==9:
                     x_speed = np.mean(np.asarray(self.pointx[4:8]) - np.asarray(self.pointx[3:7]))
                     wpose.position.x += 2 * x_speed
-                else:
-                    x_speed = np.mean(np.asarray(self.pointx[4:8])-np.asarray(self.pointx[3:7]))
-                    wpose.position.x += (x_speed-self.error_x*0.025/105)
+                    wpose.position.z = 0.05
+
 
-                wpose.position.z = 0.05
+                else:
+                    tracker.flag2 = True
+                    self.cxy_pub.publish(tracker)
+
+                    if len(self.pointx)<13:
+                        x_speed = np.mean(np.asarray(self.pointx[4:8])-np.asarray(self.pointx[3:7]))
+                        wpose.position.x += (x_speed-self.error_x*0.025/105)
+
+                    else:
+                        drop_pose = deepcopy(start_pose)
+                        drop_pose.position.x = -0.5215
+                        drop_pose.position.y = 0.2014
+                        drop_pose.position.z = 0.4102
+                        seq_y = np.arange(start_pose.position.y,drop_pose.position.y+0.03, 0.03)
+                        dx = np.linspace(start_pose.position.x,drop_pose.position.x, len(seq_y))
+                        dz = np.linspace(start_pose.position.z,drop_pose.position.z, len(seq_y))
+                        for _ in range(len(seq_y)):
+                            wpose.position.x += dx
+                            wpose.position.y += 0.03
+                            wpose.position.z += dz
+                        self.waypoints.append(deepcopy(wpose))
+                        self.arm.set_start_state_to_current_state()
+                        plan, fraction = self.arm.compute_cartesian_path(self.waypoints, 0.01, 0.0, True)
+
+                        self.phase = 2
 
 
             # Set the next waypoint to the right 0.5 meters
             else:
                 wpose.position.x -= self.error_x*0.08/105
                 wpose.position.y += self.error_y*0.04/105
-                wpose.position.z = 0.20
+                wpose.position.z = 0.15
                 #wpose.position.z = 0.4005
 
-            wpose.orientation.x = 0.4811
-            wpose.orientation.y = 0.4994
-            wpose.orientation.z = -0.5121
-            wpose.orientation.w = 0.5069
-
+            if self.phase == 1:
+                self.waypoints.append(deepcopy(wpose))
 
-            self.waypoints.append(deepcopy(wpose))
-            self.pointx.append(wpose.position.x)
-            self.pointy.append(wpose.position.y)
 
-            # Set the internal state to the current state
-            # self.arm.set_pose_target(wpose)
+                self.pointx.append(wpose.position.x)
+                self.pointy.append(wpose.position.y)
 
-            self.arm.set_start_state_to_current_state()
+                # Set the internal state to the current state
+                # self.arm.set_pose_target(wpose)
 
-            # Plan the Cartesian path connecting the waypoints
+                self.arm.set_start_state_to_current_state()
 
-            """moveit_commander.move_group.MoveGroupCommander.compute_cartesian_path(
-                    self, waypoints, eef_step, jump_threshold, avoid_collisios= True)
+                # Plan the Cartesian path connecting the waypoints
 
-               Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the
-               poses specified as waypoints. Configurations are computed for every eef_step meters;
-               The jump_threshold specifies the maximum distance in configuration space between consecutive points
-               in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed,
-               the actual RobotTrajectory.
+                """moveit_commander.move_group.MoveGroupCommander.compute_cartesian_path(
+                        self, waypoints, eef_step, jump_threshold, avoid_collisios= True)
+    
+                   Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the
+                   poses specified as waypoints. Configurations are computed for every eef_step meters;
+                   The jump_threshold specifies the maximum distance in configuration space between consecutive points
+                   in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed,
+                   the actual RobotTrajectory.
+    
+                """
+                plan, fraction = self.arm.compute_cartesian_path(self.waypoints, 0.01, 0.0, True)
 
-            """
-            plan, fraction = self.arm.compute_cartesian_path(self.waypoints, 0.01, 0.0, True)
 
             # plan = self.arm.plan()
 
@@ -247,6 +280,7 @@ def execute(self):
                 rospy.loginfo("Path execution complete.")
             else:
                 rospy.loginfo("Path planning failed")
+
         else:
             # Get the current pose so we can add it as a waypoint
             start_pose = self.arm.get_current_pose(self.end_effector_link).pose

ur5_vision.py

@@ -77,13 +77,12 @@ def image_callback(self,msg):
         #  biggest_contour = max(contour_sizes, key=lambda x: x[0])[1]
             for i, c in enumerate(cnts):
                 area = cv2.contourArea(c)
-
                 if area > 7500:
                     self.track_flag = True
                     self.cx = cx
                     self.cy = cy
                     self.error_x = self.cx - w/2
-                    self.error_y = self.cy - (h/2+130)
+                    self.error_y = self.cy - (h/2+185)
                     tracker.x = cx
                     tracker.y = cy
                     tracker.flag1 = self.track_flag

urdf/red_box.urdf

@@ -18,11 +18,11 @@
 
   <inertial>
     <origin xyz="0 0 0.02" rpy="0 0 0" />
-    <mass value="0.00001" />
+    <mass value="0.2" />
     <inertia
-      ixx="0.00004" ixy="0.0"  ixz="0.0"
-      iyy="0.00003" iyz="0.0"
-      izz="0.00001" />
+      ixx="0.01" ixy="0.0"  ixz="0.0"
+      iyy="0.01" iyz="0.0"
+      izz="0.01" />
   </inertial>
   </link>