Executing a plan

[jramadeu]

Give a high level plan like this

"id": "xyz",
"init": "initURI",
"goal": "finalURI",
"plan":[
            ["move", "turtlebot2i", "TagShelf#1"],
            ["pick", "turtlebot2i", "ShelfBody#1", "productRed"],
            ["pick", "turtlebot2i", "ShelfBody#1", "productGreen"],
            ["move", "turtlebot2i", "Waypoint_CB"],
            ["drop", "turtlebot2i", "ConveyorBeltBody", "productRed"]
       ]

The robot should move to TagShelf#1, pick productRed on ShelfBody#1 and so forth

[jramadeu]

• main tasks

• [X] robot can move to a waypoint. E.g., TahShelf#1

• [X] robot arm can plan pickup

• [X] robot arm can execute a pickup plan

• [x] add position sensors to manipulation scene

• [x] change v-rep parameters to make the robot able to pick a product #117

• [x] execute place

• [x] execute drop

• small changes

  • change reference frame on the semantic description file. With this setup collision objects don't move with the robot.
  • update scene reference frame to be compatible with URDF
  • change arm dynamic masks on v-rep scene
  • change shelf object shape composition due to a collision with the robot and the shelf bounding box
  • change child script to spawn products
  • change joints limits in vrep to match urdf (joint limits can't match exactly limits in vrep. this cause bad behaviour on v-rep pid controller).
  • added reference frame information for pick-up planning
  • correct /map to /odom tranform to map z-axis offset in V-rep
  • added products and correct positions to scene.yaml
  • turtlebot2i_warehouse/src/plan_interpreter.py
    • Correct file name
    • add function to read quaternion infomration from scene.yaml
    • add function to transform array to ros PoseStamped message

[jramadeu]

changes on branch issue-116

[jramadeu]

@albertohata, I need some help from you.

I changed something in the project and now the robot can't reach a given position. When it tries to follow the plan it goes towards the waypoint but before it arrives the robot starts to rotate about its own axis and don't reach the waypoint. Can you help me?

[albertohata]

@albertohata, I need some help from you.

I changed something in the project and now the robot can't reach a given position. When it tries to follow the plan it goes towards the waypoint but before it arrives the robot starts to rotate about its own axis and don't reach the waypoint. Can you help me?

Sure. I will take a look. Probably some parameters of the navigation need to be adjusted.

[albertohata]

@albertohata, I need some help from you. I changed something in the project and now the robot can't reach a given position. When it tries to follow the plan it goes towards the waypoint but before it arrives the robot starts to rotate about its own axis and don't reach the waypoint. Can you help me?

Sure. I will take a look. Probably some parameters of the navigation need to be adjusted.

@jramadeu I tested in my machine and it worked correctly. Can you list the steps that you followed to run the move task? My guess is that some of the launch file is not properly loading the navigation configuration files in .../scott-eu/simulation-ros/src/turtlebot2i/turtlebot2i_navigation/config

[jramadeu]

On branch issue_116 and follow this steps:

1. initialize roscore
2. open v-rep
3. load and run /turtlebot2i_description/v-rep_model/warehourse_turtlebot2i_v3_recharge.ttt
4. roslaunch turtlebot2i_bringup turtlebot2i_full.launch
5. pyhton src/plan_interpreter.py

If it still doesn't show any issue, try to change on the plan.json:

"id": "xyz", "init": "initURI", "goal": "finalURI", "plan":[ ["move", "turtlebot2i", "Waypoint_SH"], ["pick", "turtlebot2i", "ShelfBody", "productRed"], ["pick", "turtlebot2i", "ShelfBody", "productGreen"], ["move", "turtlebot2i", "Waypoint_CB#1"], ["drop", "turtlebot2i", "ConveyorBeltBody#1", "productRed"] ] }

[albertohata]

@jramadeu can you test the robot navigation again? I've updated the planner configuration.

[jramadeu]

Now the plan execution is more reliable, The robot is able to execute the pick and drop, although sometimes it fails. More important, most of time it will work.

Important! Due to tolerances with robot's navigation it will just pick one object. If we improve that, it will be able to pick more than one.

There're still things to be improved like: gripper pitch angle to grasp and robot navigation.

@albertohata, I notice you're keeping you're modification on task_manager here. I think this branch has too many commits and it's time to merge into master. After we merge we can create issues to keep working on plan execution, grasp pitch angle and improving the navigation. What do you think?

[albertohata]

Now the plan execution is more reliable, The robot is able to execute the pick and drop, although sometimes it fails. More important, most of time it will work.

Important! Due to tolerances with robot's navigation it will just pick one object. If we improve that, it will be able to pick more than one.

There're still things to be improved like: gripper pitch angle to grasp and robot navigation.

@albertohata, I notice you're keeping your modification on task_manager here. I think this branch has too many commits and it's time to merge into master. After we merge we can create issues to keep working on plan execution, grasp pitch angle and improving the navigation. What do you think?

Yes, I agree. I also have the same feeling. The planning part should be moved to another issue as it still has many work to do.

[jramadeu]

@albertohata, I was checking this issue and the respective branch. I found something that may deserves you attention.

I made the merge on master on 20Dec 2018 and the last commit merged was made on the same day, check here. But after that, you also pushed some commit to the same branch on 21Dec 2018 and 09Jan 2019, check here.

Therefore, these two recent commit were not merged. It's your decision to merge them or to just cherry-pick them to another branch.

[albertohata]

@albertohata, I was checking this issue and the respective branch. I found something that may deserves you attention.

I made the merge on master on 20Dec 2018 and the last commit merged was made on the same day, check here. But after that, you also pushed some commit to the same branch on 21Dec 2018 and 09Jan 2019, check here.

Therefore, these two recent commit were not merged. It's your decision to merge them or to just cherry-pick them to another branch.

@jramadeu If you don't mind, I will reopen this issue and continue working on the task worker and task manager. Otherwise I can create another issue/branch and continue working from there.

[jramadeu]

I don't mind at all.