Adding an object to pick

In order to check how the simulation works I added a new box, first I recycle the yellow box and place it near the other one. To do so, I edit /root/spawn_objects_small.sh and add the following line:

ros2 run ros2srrc_execution SpawnObject.py --package "ros2srrc_irb120_gazebo" --urdf "yellow_box_small.urdf" --name "yellow_box_small2" --x 0.6 --y 0.5 --z 1.01;

In order to make the UR pick it up I added the actions to /root/demo_grasp_ros2_ur5.sh:

                # YELLOW BOX 2 Pick and place
                    ros2 run ros2srrc_execution ExecuteProgram.py package:=ros2srrc_execution program:=ur5_robotiq85_grasp_yellow_pick2;

                    ros2 action send_goal -f /Move ros2srrc_data/action/Move "{action: 'MoveG', moveg: 20.0, speed: 0.1}";

                    ros2 service call /ATTACHLINK linkattacher_msgs/srv/AttachLink "{model1_name: 'ur5', link1_name: 'EE_robotiq_2f85', model2_name: 'yellow_box_small2', link2_name: 'yellow_box_small'}"

                    ros2 run ros2srrc_execution ExecuteProgram.py package:=ros2srrc_execution program:=ur5_robotiq85_grasp_yellow_place;

                    ros2 service call /DETACHLINK linkattacher_msgs/srv/DetachLink "{model1_name: 'ur5', link1_name: 'EE_robotiq_2f85', model2_name: 'yellow_box_small2', link2_name: 'yellow_box_small'}"

            

This uses a program called ur5_robotiq85_grasp_yellow_pick2.yaml that we need to create in the following path: root/dev_ws/src/ros2_SimRealRobotControl/ros2srrc_execution/programs:

or download this file and add it: See YAML file

    
Specifications:
  Robot: "ur5"
  EndEffector: "ParallelGripper"
  EELink: "EE_robotiq_2f85"
  Objects: ["blue_sphere_small", "red_box_small", "yellow_box_small", "green_cylinder_small" , "yellow_box_small2"]
  
Sequence: 
  - Step: 1
    Name: "[Grasp-Demo]: MoveJ - Go to HomePosition."
    Type: "MoveJ"
    Speed: 1.0
    Delay: 0.0
    Input:
      joint1: 0.0
      joint2: -90.0
      joint3: 60.0
      joint4: -60.0
      joint5: -90.0
      joint6: 0.0
  - Step: 2
    Name: "[Grasp-Demo]: Open gripper"
    Type: "ParallelGripper"
    Action: "OPEN"
    Delay: 0.0
  - Step: 3
    Name: "[Grasp-Demo]: MoveL to yellow box APROX point"
    Type: "MoveL"
    Speed: 1.0
    Delay: 0.0
    Input:
      x: 0.175
      y: 0.392
      z: -0.32
  - Step: 4
    Name: "[Grasp-Demo]: MoveROT 90º in roll"
    Type: "MoveROT"
    Speed: 1.0
    Delay: 0.0
    Input:
      yaw: 0.0
      pitch: 0.0
      roll: 90.0
  - Step: 5
    Name: "[Grasp-Demo]: MoveL to yellow box PICK point"
    Type: "MoveL"
    Speed: 0.3
    Delay: 0.0
    Input:
      x: 0.0
      y: 0.0
      z: -0.08

     

Now we need to make colcon build. Go to root/dev_ws/ and execute:

colcon build --packages-select ros2srrc_execution

And restart the simulation again.

Link video.