[pick_place_demo.py] cannot grasp the cube #31
Comments
|
Thanks, how reproducible is this? Does it fail every time? Or sometimes pass as mentioned in this issue #20 |
|
I tried like 5 times, it was never successful. One thing is that I am using integrated GPU (Intel® HD Graphics 620). Here is my environment setup: Environment SetupMake sure Ubuntu 18.04 and ROS Melodic are installed. We are using this version because it is also installed on the robot and it works. (Currently only ROS Indigo is supported as Debian packages, but there are git branches for Ubuntu 18.04/ROS Melodic and both are installed on the onboard computer of the robot.) Installing fetch_ros & fetch_gazebo from sourceInstall the dependencies for Fetch Robotics: Create a catkin workspace: http://wiki.ros.org/catkin/Tutorials/create_a_workspace In the carkin workspace In your carkin workspace Test your installation in an empty worldGazebo demoInstall dependencies: demo: |
|
I also made changes mentioned in PR #26. |
|
We have a new Intern: @ejichen who is working on updating our demos with the Fetch in Simulation and on the Real Robot. She started this week and will work on a new cube picking demo. I'll assign this to her to look into.
|
|
Hi @umhan35, On the first try, the Gazebo window shows that the robot flickered a little bit after it moved the arm (0:05 in the video). The movement shifted the robot, resulting in the mismatch of the robot frame and the planning scene frame (shows in the rviz window). The second try didn't succeed because the previous movement made some changes to the position of the cube. However, if you let the simulation run for a while, you could see the pick and place demo succeed with the third try (since the script is a while loop of pick and place). |
|
@umhan35 I've made two URDF changes.
@ejichen has been using 1. which is a quick fix, but I will make a PR with 2. which adds the caster wheels to the model |
|
Actually, I before committing the changes, I went to record before and after videos. And surprisingly the fetch was able to grasp the cube, using the ccb5ad3 and the fetch-ros from debians https://github.com/fetchrobotics/fetch_ros/releases/tag/0.8.0. I didn't use fetch_ros from source- the versions from source upgraded the fetch_ikfast_plugin ZebraDevs/fetch_ros#107 to fix deprecation warnings with the next version of moveit. |
|
I'd say the behavior is not consistent. I am not sure if it is a Gazebo issue. |
|
The behaviour isn’t expected to be consistent, it’s running MoveIt! with random sampling based motion planners. But, if we make the URDF changes to stablize the base (the real Robot is very stable and doesn‘t wiggle or tilt when moving the arm) then it should behave closer to the real thing. In #37, I’ve posted videos of Indigo & Kinetic and the inertia/CoM values visually look different- despite not being changed. |
This adds 4 casters to the gazebo model, and updates the physics params. This should fix ZebraDevs#31. This is related to ZebraDevs#37
Related to issue #30, I tried to run the pick_place_demo.py without the navigation part:
It seems there are some error detecting the cube.
Grasping:
Picking:
Here is the full log:
$ roslaunch fetch_gazebo_demo pick_place_demo.launch
... logging to /home/zhao/.ros/log/e097f2ea-3066-11e9-8d6a-04d3b0082825/roslaunch-zhao1804-28988.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://zhao1804:42179/
SUMMARY
PARAMETERS
NODES
/
basic_grasping_perception (simple_grasping/basic_grasping_perception)
demo (fetch_gazebo_demo/pick_place_demo.py)
move_group (moveit_ros_move_group/move_group)
ROS_MASTER_URI=http://localhost:11311
running rosparam delete /move_group/sensors
ERROR: parameter [/move_group/sensors] is not set
process[move_group-1]: started with pid [29026]
process[basic_grasping_perception-2]: started with pid [29027]
process[demo-3]: started with pid [29028]
[ INFO] [1550155420.247534970]: Loading robot model 'fetch'...
[ INFO] [1550155420.249758237]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1550155420.697570069, 7.515000000]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ WARN] [1550155420.714784863, 7.516000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1550155421.051486137, 7.557000000]: Publishing maintained planning scene on 'monitored_planning_scene'
[ INFO] [1550155421.055649797, 7.558000000]: MoveGroup debug mode is ON
Starting planning scene monitors...
[ INFO] [1550155421.055705277, 7.558000000]: Starting planning scene monitor
[ INFO] [1550155421.057952369, 7.559000000]: Listening to '/planning_scene'
[ INFO] [1550155421.057998972, 7.559000000]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1550155421.061246577, 7.560000000]: Listening to '/collision_object' using message notifier with target frame 'base_link '
[ INFO] [1550155421.065676134, 7.560000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1550155421.066743463, 7.560000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1550155421.174592184, 7.576000000]: Listening to '/attached_collision_object' for attached collision objects
Planning scene monitors started.
[ INFO] [1550155421.247784878, 7.585000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1550155421.316253356, 7.594000000]: Using planning interface 'OMPL'
[ INFO] [1550155421.326796233, 7.595000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1550155421.327774670, 7.595000000]: Param 'start_state_max_bounds_error' was set to 0.1
[ INFO] [1550155421.328958543, 7.596000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550155421.332633334, 7.597000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1550155421.333655127, 7.597000000]: Param 'jiggle_fraction' was set to 0.05
[ INFO] [1550155421.335969232, 7.597000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1550155421.336141642, 7.597000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1550155421.336213837, 7.597000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1550155421.336248030, 7.597000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1550155421.336663577, 7.597000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1550155421.336703310, 7.597000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1550155421.637563825, 7.681000000]: Added FollowJointTrajectory controller for arm_controller
[ INFO] [1550155421.927068015, 7.761000000]: Added FollowJointTrajectory controller for arm_with_torso_controller
[ INFO] [1550155422.260822953, 7.837000000]: Added GripperCommand controller for gripper_controller
[ INFO] [1550155422.261156981, 7.837000000]: Returned 3 controllers in list
[ INFO] [1550155422.343763647, 7.853000000]: Trajectory execution is managing controllers
Loading 'move_group/ApplyPlanningSceneService'...
Loading 'move_group/ClearOctomapService'...
Loading 'move_group/MoveGroupCartesianPathService'...
Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
Loading 'move_group/MoveGroupGetPlanningSceneService'...
Loading 'move_group/MoveGroupKinematicsService'...
Loading 'move_group/MoveGroupMoveAction'...
Loading 'move_group/MoveGroupPickPlaceAction'...
Loading 'move_group/MoveGroupPlanService'...
Loading 'move_group/MoveGroupQueryPlannersService'...
Loading 'move_group/MoveGroupStateValidationService'...
[ INFO] [1550155422.619851838, 7.897000000]:
[ INFO] [1550155422.619959322, 7.897000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1550155422.619986510, 7.897000000]: MoveGroup context initialization complete
You can start planning now!
[INFO] [1550155425.182025, 8.641000]: Waiting for head_controller...
[INFO] [1550155425.332129, 8.685000]: Waiting for get_planning_scene
[INFO] [1550155425.380074, 8.699000]: Connecting to pickup action...
[INFO] [1550155425.683999, 8.812000]: ...connected
[INFO] [1550155425.685496, 8.812000]: Connecting to place action...
[INFO] [1550155425.991732, 8.929000]: ...connected
[INFO] [1550155426.291533, 9.044000]: Waiting for basic_grasping_perception/find_objects...
[ INFO] [1550155426.638234411, 9.176000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1550155426.638568602, 9.176000000]: Goal constraints are already satisfied. No need to plan or execute any motions
[INFO] [1550155429.294401, 10.189000]: Picking object...
[WARN] [1550155429.744109, 10.346000]: ObjectManager: object0 not added yet
[WARN] [1550155429.750793, 10.347000]: ObjectManager: surface0 not added yet
position:
x: 0.800304591656
y: 0.128784805536
z: 0.719509720802
orientation:
x: 0.00225892546587
y: -0.000417060917243
z: 0.99982714653
w: -0.0184499844909 type: 1
dimensions: [0.05848413705825806, 0.05982690304517746, 0.06015282869338989]
[INFO] [1550155430.282192, 10.547000]: Beginning to pick.
[ INFO] [1550155430.308622788, 10.555000000]: Planning attempt 1 of at most 1
[ INFO] [1550155430.335774344, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550155430.335932737, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550155430.335997754, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550155430.336049304, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550155430.336102672, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550155430.336161552, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550155430.336230476, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550155430.336293907, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550155430.336358307, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550155430.336417054, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550155430.336463869, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550155430.336524922, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550155430.336571700, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550155430.336630808, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550155430.336684226, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550155430.336746206, 10.560000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550155430.337327785, 10.561000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550155430.337539936, 10.561000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550155430.347837217, 10.562000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155430.358707999, 10.568000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.359482408, 10.568000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155430.364262208, 10.572000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.370562400, 10.572000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155430.373513745, 10.575000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155430.383921179, 10.576000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.392562350, 10.583000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.398216377, 10.585000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.399175114, 10.586000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155430.420301382, 10.590000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155430.448059141, 10.600000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155431.431005965, 10.805000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155431.448205953, 10.806000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155431.456181845, 10.807000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155436.993896554, 11.798000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1550155436.999268899, 11.799000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1550155437.024884951, 11.805000000]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1550155437.027349463, 11.806000000]: Solution found in 0.028532 seconds
[ INFO] [1550155437.076598579, 11.814000000]: SimpleSetup: Path simplification took 0.047470 seconds and changed from 4 to 2 states
[ INFO] [1550155437.084802118, 11.814000000]: Found successful manipulation plan!
[ INFO] [1550155440.007407178, 12.296000000]: Manipulation plan 12 failed at stage 'approach & translate' on thread 0
[ INFO] [1550155440.018842795, 12.298000000]: Manipulation plan 13 failed at stage 'approach & translate' on thread 2
[ INFO] [1550155440.019213599, 12.298000000]: Pickup planning completed after 9.710337 seconds
[ WARN] [1550155463.913899004, 19.668000000]: Controller arm_controller failed with error code GOAL_TOLERANCE_VIOLATED
[ WARN] [1550155463.914225237, 19.668000000]: Controller handle arm_controller reports status ABORTED
[ INFO] [1550155463.914328516, 19.668000000]: Completed trajectory execution with status ABORTED ...
[ERROR] [1550155464.062538, 19.698000]: Pick failed during execution, attempting to cleanup.
[INFO] [1550155464.069791, 19.698000]: Pick did not grab object, try again...
[ INFO] [1550155464.102480738, 19.705000000]: Planning attempt 1 of at most 1
[ INFO] [1550155464.103529129, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550155464.103665656, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1550155464.103727427, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 2
[ INFO] [1550155464.103792350, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 3
[ INFO] [1550155464.103840080, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 4
[ INFO] [1550155464.103873440, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 5
[ INFO] [1550155464.103908786, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 6
[ INFO] [1550155464.103942663, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 7
[ INFO] [1550155464.103976199, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 8
[ INFO] [1550155464.104010031, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 9
[ INFO] [1550155464.104041310, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 10
[ INFO] [1550155464.104075503, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 11
[ INFO] [1550155464.104110760, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 12
[ INFO] [1550155464.104143595, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 13
[ INFO] [1550155464.104175470, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 14
[ INFO] [1550155464.104207791, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 15
[ INFO] [1550155464.104238852, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 16
[ INFO] [1550155464.104286326, 19.705000000]: Added plan for pipeline 'pick'. Queue is now of size 17
[ INFO] [1550155464.115045678, 19.705000000]: Manipulation plan 5 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155464.121413457, 19.706000000]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155464.127622271, 19.706000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155464.134631177, 19.708000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155464.141600112, 19.708000000]: Manipulation plan 6 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1550155464.148302253, 19.709000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155464.149319629, 19.709000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155464.155938859, 19.709000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155464.163443144, 19.709000000]: Manipulation plan 14 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155464.180421518, 19.710000000]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1550155464.183416335, 19.710000000]: Manipulation plan 8 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155464.189920666, 19.711000000]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1550155465.402118125, 19.831000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155465.412926059, 19.832000000]: Manipulation plan 9 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155465.422259278, 19.834000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1550155471.756547482, 20.682000000]: Found a contact between 'object0' (type 'Object') and 'r_gripper_finger_link' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1550155471.756716465, 20.682000000]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1550155471.756875507, 20.682000000]: Start state appears to be in collision with respect to group arm
[ INFO] [1550155471.757233064, 20.682000000]: Found a valid state near the start state at distance 0.086531 after 0 attempts
[ INFO] [1550155471.758017345, 20.682000000]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1550155471.758566091, 20.682000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1550155471.773592136, 20.684000000]: RRTConnect: Created 5 states (3 start + 2 goal)
[ INFO] [1550155471.773683964, 20.684000000]: Solution found in 0.015258 seconds
[ INFO] [1550155471.824327334, 20.691000000]: SimpleSetup: Path simplification took 0.050551 seconds and changed from 4 to 2 states
[ INFO] [1550155471.830048520, 20.692000000]: Planning adapters have added states at index positions: [ 0 ]
[ INFO] [1550155471.837971940, 20.692000000]: Found successful manipulation plan!
[ INFO] [1550155473.663829588, 20.963000000]: Manipulation plan 13 failed at stage 'approach & translate' on thread 0
[ INFO] [1550155474.116754124, 21.039000000]: Manipulation plan 12 failed at stage 'approach & translate' on thread 3
[ INFO] [1550155474.116946183, 21.039000000]: Pickup planning completed after 10.014108 seconds
^C
The text was updated successfully, but these errors were encountered: