search

SoMa-Project / ec_grasp_planner

Grasp Planner Based on Environmental Constraint Exploitation
BSD 3-Clause "New" or "Revised" License
7 stars 3 forks source link

running the demo #5

Closed joaobimbo closed 7 years ago

joaobimbo commented 7 years ago

I'm trying to run the demo: "Kuka Arm in Gazebo Simulation with TRIK Controller" Here are my issues:

cerdogan commented 7 years ago

Hello Joao,

I updated the README for hybrid_automaton_manager_kuka with the TRIK controller link: https://github.com/SoMa-Project/trik_controller.

Let's discuss the other points once you install it and can follow the rest of the instructions in the hybrid_automaton_manager_kuka.

One thing you could do before running the full demo is to run one of the test files. For instance, you can check out hybrid_automaton_manager_kuka/tests/testTRIK.cpp. It has instructions at the top comments and it would only test the hybrid automaton manager and TRIK.

joaobimbo commented 7 years ago

Thanks Can. Any reason you're sharing the binaries instead of source?

cerdogan commented 7 years ago

TRIK controller is a part of Ocado's proprietary code and therefore at the moment, they can not share it. There is a pseudo-code description of what it does in its README.

joaobimbo commented 7 years ago

Hi. Still trying to run the demo. Now, when running the command rosrun ecto_rbo_yaml plasm_yaml_ros_node.pyrospack find ec_grasp_planner/data/geometry_graph_example3.yaml --debug I get the output:

$ rosrun ecto_rbo_yaml plasm_yaml_ros_node.py `rospack find ec_grasp_planner`/data/geometry_graph_example3.yaml --debug
[ INFO] [1498212836.388665643]: Initialized ROS. node_name: /plasm_yaml_ros_node
shutdown request: new node registered with same name
 Adding tf_send_wall (type: ecto_rbo_pcl::BroadcastTF, class: ecto_rbo_pcl::BroadcastTF)
 Adding edge_grasp_planner (type: ecto_rbo_grasping::CreateGeometryGraph, class: ecto_rbo_grasping::CreateGeometryGraph)
 Adding publish_graph (type: ecto_rbo_grasping::PublishGeometryGraph, class: ecto_rbo_grasping::PublishGeometryGraph)
 Adding filter_closest_object (type: ecto_rbo_pcl::ExtractClosestCluster, class: ecto_rbo_pcl::ExtractClosestCluster)
 Adding region_growing_rgb (type: ecto_rbo_pcl::RegionGrowingRGB, class: ecto_rbo_pcl::RegionGrowingRGB)
 Adding tf_send_table (type: ecto_rbo_pcl::BroadcastTF, class: ecto_rbo_pcl::BroadcastTF)
 Adding help_remove_nans (type: ecto_rbo_pcl::RemoveNaNFromPointCloud, class: ecto_rbo_pcl::RemoveNaNFromPointCloud)
 Adding tf_send_object (type: ecto_rbo_pcl::BroadcastTF, class: ecto_rbo_pcl::BroadcastTF)
 Adding filter_wall_by_size (type: ecto_rbo_pcl::FilterClustersBySize, class: ecto_rbo_pcl::FilterClustersBySize)
 Adding merge_clusters (type: ecto_rbo_pcl::MergeClusters, class: ecto_rbo_pcl::MergeClusters)
Warn: cell flood_fill does not have key rviz_prefix
 Adding flood_fill (type: ecto_rbo_pcl::FloodFill, class: ecto_rbo_pcl::FloodFill)
 Adding tf_to_vector_table (type: ecto_rbo_pcl::TF2Vector, class: ecto_rbo_pcl::TF2Vector)
 Adding tf_send_camera (type: ecto_rbo_pcl::BroadcastTF, class: ecto_rbo_pcl::BroadcastTF)
 Adding filter_by_distance (type: ecto_rbo_pcl::FilterClustersBySize, class: ecto_rbo_pcl::FilterClustersBySize)
 Adding bbox_to_axis (type: ecto_rbo_pcl::TFs2Vectors, class: ecto_rbo_pcl::TFs2Vectors)
 Adding calculate_centroids (type: ecto_rbo_pcl::Centroid, class: ecto_rbo_pcl::Centroid)
 Adding table_fits (type: ecto_rbo_pcl::PlaneFits, class: ecto_rbo_pcl::PlaneFits)
 Adding create_tf (type: ecto_rbo_pcl::CreateTF, class: ecto_rbo_pcl::CreateTF)
 Adding filter_wall_by_normal (type: ecto_rbo_pcl::FilterClustersByNormals, class: ecto_rbo_pcl::FilterClustersByNormals)
 Adding wall_grasps (type: ecto_rbo_grasping::WallGrasps, class: ecto_rbo_grasping::WallGrasps)
 Adding filter_object_by_size (type: ecto_rbo_pcl::FilterClustersBySize, class: ecto_rbo_pcl::FilterClustersBySize)
 Adding ros_pc_subscriber (type: ecto_sensor_msgs::Subscriber_PointCloud2, class: ecto_sensor_msgs::Subscriber_PointCloud2)
 Adding planar_manifolds (type: ecto_rbo_grasping::ManifoldsPlanar, class: ecto_rbo_grasping::ManifoldsPlanar)
 Adding take_first_tf (type: ecto_rbo_pcl::Vector2TF, class: ecto_rbo_pcl::Vector2TF)
 Adding msg_to_pcl (type: ecto_rbo_pcl::Message2PointCloud, class: ecto_rbo_pcl::Message2PointCloud)
 Adding positioning_motions (type: ecto_rbo_grasping::PositioningMotions, class: ecto_rbo_grasping::PositioningMotions)
 Adding cliff_grasps (type: ecto_rbo_grasping::CliffGrasps, class: ecto_rbo_grasping::CliffGrasps)
 Adding bbox_fits (type: ecto_rbo_pcl::PlaneFits2D, class: ecto_rbo_pcl::PlaneFits2D)
 Adding create_top_down_grasps (type: ecto_rbo_grasping::CreateGrasps, class: ecto_rbo_grasping::CreateGrasps)
 Adding wall_fit (type: ecto_rbo_pcl::PlaneFits, class: ecto_rbo_pcl::PlaneFits)
 Adding crop_box (type: ecto_rbo_pcl::PassThroughOrganized, class: ecto_rbo_pcl::PassThroughOrganized)
 Adding normal_estimation (type: ecto_rbo_pcl::NormalEstimationIntegral, class: ecto_rbo_pcl::NormalEstimationIntegral)
 Adding wrap_in_vector_table (type: ecto_rbo_pcl::WrapVector3fInVector, class: ecto_rbo_pcl::WrapVector3fInVector)
 Connecting wall_fit.transform_biggest >> tf_send_wall.transform
 Connecting create_top_down_grasps.pregrasp_messages >> edge_grasp_planner.surface_pregrasp_messages
 Connecting planar_manifolds.pushing_pregrasp_messages >> edge_grasp_planner.pushing_pregrasp_messages
 Connecting cliff_grasps.cliff_pregrasp_messages >> edge_grasp_planner.edge_pregrasp_messages
 Connecting planar_manifolds.manifolds >> edge_grasp_planner.surface_grasp_manifolds
 Connecting planar_manifolds.manifolds >> edge_grasp_planner.pushing_motion_manifolds
 Connecting wall_grasps.wall_pregrasp_messages >> edge_grasp_planner.wall_pregrasp_messages
 Connecting wall_grasps.wall_manifolds >> edge_grasp_planner.wall_grasp_manifolds
 Connecting cliff_grasps.cliff_manifolds >> edge_grasp_planner.edge_grasp_manifolds
 Connecting positioning_motions.positioning_manifolds >> edge_grasp_planner.positioning_motion_manifolds
 Connecting positioning_motions.positioning_pregrasp_messages >> edge_grasp_planner.positioning_pregrasp_messages
 Connecting edge_grasp_planner.graph_message >> publish_graph.graph_message
 Connecting crop_box.output >> filter_closest_object.input
 Connecting table_fits.transform_biggest >> filter_closest_object.transform
 Connecting filter_object_by_size.filtered_clusters >> filter_closest_object.clusters
 Connecting help_remove_nans.indices >> region_growing_rgb.indices
 Connecting crop_box.output >> region_growing_rgb.input
 Connecting table_fits.transform_biggest >> tf_send_table.transform
 Connecting crop_box.output >> help_remove_nans.input
 Connecting take_first_tf.output >> tf_send_object.transform
 Connecting crop_box.output >> filter_wall_by_size.input
 Connecting flood_fill.clusters >> filter_wall_by_size.clusters
 Connecting region_growing_rgb.output >> merge_clusters.cluster_0
 Connecting filter_by_distance.filtered_clusters >> merge_clusters.cluster_1
 Connecting crop_box.output >> flood_fill.input
 Connecting normal_estimation.output >> flood_fill.normals
 Connecting table_fits.transform_biggest >> tf_to_vector_table.transform
 Connecting create_tf.transform >> tf_send_camera.transform
 Connecting crop_box.output >> filter_by_distance.input
 Connecting table_fits.transform_biggest >> filter_by_distance.transform
 Connecting flood_fill.clusters >> filter_by_distance.clusters
 Connecting bbox_fits.transforms >> bbox_to_axis.transforms
 Connecting crop_box.output >> filter_object_by_size.input
 Connecting merge_clusters.merged_clusters >> filter_object_by_size.clusters
 Connecting flood_fill.clusters >> table_fits.clusters
 Connecting crop_box.output >> table_fits.input
 Connecting normal_estimation.output >> table_fits.normals
 Connecting crop_box.output >> filter_wall_by_normal.input
 Connecting normal_estimation.output >> filter_wall_by_normal.normals
 Connecting table_fits.transform_biggest >> filter_wall_by_normal.transform
 Connecting filter_wall_by_size.filtered_clusters >> filter_wall_by_normal.clusters
 Connecting crop_box.output >> wall_grasps.input
 Connecting normal_estimation.output >> wall_grasps.normals
 Connecting table_fits.polygons >> wall_grasps.polygons
 Connecting table_fits.bounded_models >> wall_grasps.bounded_planes
 Connecting crop_box.output >> calculate_centroids.input
 Connecting filter_closest_object.closest_cluster >> calculate_centroids.clusters
 Connecting msg_to_pcl.output >> crop_box.input
 Connecting crop_box.output >> planar_manifolds.input
 Connecting normal_estimation.output >> planar_manifolds.normals
 Connecting table_fits.bounded_model_biggest >> planar_manifolds.bounded_planes
 Connecting bbox_fits.transforms >> take_first_tf.input
 Connecting ros_pc_subscriber.output >> msg_to_pcl.input
 Connecting crop_box.output >> positioning_motions.input
 Connecting normal_estimation.output >> positioning_motions.normals
 Connecting crop_box.output >> cliff_grasps.input
 Connecting normal_estimation.output >> cliff_grasps.normals
 Connecting table_fits.polygon_biggest >> cliff_grasps.polygons
 Connecting table_fits.bounded_model_biggest >> cliff_grasps.bounded_planes
 Connecting crop_box.output >> bbox_fits.input
 Connecting filter_closest_object.closest_cluster >> bbox_fits.clusters
 Connecting tf_to_vector_table.vector >> bbox_fits.normal
 Connecting msg_to_pcl.header >> create_top_down_grasps.header
 Connecting bbox_to_axis.vectors >> create_top_down_grasps.roll_vectors
 Connecting wrap_in_vector_table.vector >> create_top_down_grasps.approach_vectors
 Connecting calculate_centroids.centroids >> create_top_down_grasps.positions
 Connecting crop_box.output >> wall_fit.input
 Connecting filter_wall_by_normal.filtered_clusters >> wall_fit.clusters
 Connecting normal_estimation.output >> wall_fit.normals
 Connecting crop_box.output >> normal_estimation.input
 Connecting tf_to_vector_table.vector >> wrap_in_vector_table.input
Couldn't add debugging cell to visualize output of cell merge_clusters.
[INFO] [WallTime: 1498212836.455653] [0.000000] Plasm initialized: ['/home/joao/ros_soma/src/ec_grasp_planner/ec_grasp_planner/data/geometry_graph_example3.yaml']
[ INFO] [1498212836.460431765]: Subscribed to topic:/camera/depth_registered/points [queue_size: 2][tcp_nodelay: 0]
[ INFO] [1498212836.652316030, 973.993000000]: publishing to topic:/msg_to_pcl/output
[ INFO] [1498212837.364350321, 974.682000000]: RegionGrowingRGB found 11 segments.
[pcl::IntegralImageNormalEstimation::setInputCloud] Input dataset is not organized (height = 1).
[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).
[ INFO] [1498212837.375727734, 974.693000000]: publishing to topic:/region_growing_rgb/output
[ INFO] [1498212837.379013326, 974.697000000]: publishing to topic:/help_remove_nans/output
[ INFO] [1498212837.380727700, 974.698000000]: publishing to topic:/crop_box/output
[ INFO] [1498212837.382870966, 974.699000000]: Computing clusters ...
Segmentation fault (core dumped)

Any idea why this is happening?

cerdogan commented 7 years ago

My guess is vision code is not running. It could happen that you need to run this code twice, rarely.

Are you following the steps as shown in this video: https://youtu.be/myx-IeDayrc?

cerdogan commented 7 years ago

By vision code, I meant the simulation of the camera in rviz.

joaobimbo commented 7 years ago

no... it's not running properly.

[ INFO] [1498220843.275519398, 215.073000000]: RegionGrowingRGB found 10 segments.
[pcl::IntegralImageNormalEstimation::setInputCloud] Input dataset is not organized (height = 1).
[pcl::IntegralImageNormalEstimation::initCompute] Input dataset is not organized (height = 1).
[ INFO] [1498220843.289030269, 215.082000000]: publishing to topic:/region_growing_rgb/output
[ INFO] [1498220843.293461515, 215.087000000]: publishing to topic:/help_remove_nans/output
[ INFO] [1498220843.295763740, 215.088000000]: publishing to topic:/crop_box/output
[ INFO] [1498220843.297348032, 215.088000000]: Computing clusters ...
Segmentation fault (core dumped)

What version of PCL are you using? Mine is 1.7

joaobimbo commented 7 years ago

For some reason, the normal_estimation needs the pointcloud to be "organized" (have a width and height). I did a dirty fix inside here where I copy the cloud and change the height and width (the original one is const)

        boost::shared_ptr < ::pcl::PointCloud <Point> > cloud(new ::pcl::PointCloud <Point> );
        *cloud=*input;
        cloud->height=240;
        cloud->width=320;
        impl.setInputCloud(cloud);
cerdogan commented 7 years ago

We use PCL 1.7 as well.

joaobimbo commented 7 years ago

Okay, clean PC, clean install, running the demo I sometimes get 

soma@soma-iit:~/ros_soma$ rosrun ec_grasp_planner planner.py --grasp WallGrasp --object_frame object --ros_service_call --rviz --handarm RBOHand2Kuka
Traceback (most recent call last):
  File "/home/soma/ros_soma/src/ec_grasp_planner/ec_grasp_planner/src/planner.py", line 466, in <module>
    main(**vars(args))
  File "/home/soma/ros_soma/src/ec_grasp_planner/ec_grasp_planner/src/planner.py", line 381, in main
    tf_listener.waitForTransform(object_frame, robot_base_frame, rospy.Time(), rospy.Duration(10.0))
  File "/opt/ros/indigo/lib/python2.7/dist-packages/tf/listener.py", line 76, in waitForTransform
    raise tf2_ros.TransformException(error_msg or "no such transformation: \"%s\" -> \"%s\"" % (source_frame, target_frame))
tf2.TransformException: canTransform: target_frame object does not exist.

Some other times I get:

tf2.TransformException: Lookup would require extrapolation into the past.  Requested time 227.624000000 but the earliest data is at time 228.126000000, when looking up transform from frame [world] to frame [object]

Sometimes it works for surface grasp.

Running the WallGrasp doesn't work. I get the following from the hybrid_automaton_manager_kuka:

soma@soma-iit:~/ros_soma$ rosrun hybrid_automaton_manager_kuka hybrid_automaton_manager_kuka
[ INFO] [1507912017.796564732]: Ready to manage hybrid automatons for the Kuka system
[ForceTorqueSensor.deserialize] WARN: The attribute frame_id for the force torque sensor is not implemented! (/home/soma/code/hybrid-automaton-library/src/ForceTorqueSensor.cpp:83)
[ForceTorqueSensor.deserialize] WARN: The attribute frame_id for the force torque sensor is not implemented! (/home/soma/code/hybrid-automaton-library/src/ForceTorqueSensor.cpp:83)
[HybridAutomaton._activeCurrentControlMode] INFO: Current mode: Mode0 (/home/soma/code/hybrid-automaton-library/src/HybridAutomaton.cpp:393)
goal R:  0.003688 -0.999987  0.003453
 0.540211  0.004898  0.841515
-0.841521 -0.001239  0.540222
goal t: 0.659577
-0.350955
0.555792
[ INFO] [1507912050.851408318, 32.633000000]: Loading robot model 'iiwa7_kinect_ft'...
[ INFO] [1507912050.851527807, 32.633000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1507912051.037278010, 32.790000000]: Loading robot model 'iiwa7_kinect_ft'...
[ INFO] [1507912051.037380196, 32.790000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
Initializing psm
[ INFO] [1507912051.117143590, 32.850000000]: Loading robot model 'iiwa7_kinect_ft'...
[ INFO] [1507912051.117171322, 32.850000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1507912051.260350750, 32.971000000]: Loading robot model 'iiwa7_kinect_ft'...
[ INFO] [1507912051.260397913, 32.971000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1507912051.582229998, 33.237000000]: Starting scene monitor
[ INFO] [1507912051.585492838, 33.237000000]: Listening to '/planning_scene'
[ INFO] [1507912051.585555881, 33.237000000]: Starting world geometry monitor
[ INFO] [1507912051.589587788, 33.240000000]: Listening to '/collision_object' using message notifier with target frame '/world '
[ INFO] [1507912051.592631337, 33.244000000]: Listening to '/planning_scene_world' for planning scene world geometry
[ WARN] [1507912051.593162951, 33.244000000]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[ INFO] [1507912051.721523190, 33.352000000]: Listening to '/attached_collision_object' for attached collision objects
Collision World Objects:
     box
     table

Attached Bodies:

iiwa_joint_1: 9.48632e-06
iiwa_joint_2: 2.09173e-06
iiwa_joint_3: 8.33015e-06
iiwa_joint_4: -3.26142e-06
iiwa_joint_5: -3.0366e-06
iiwa_joint_6: 1.97441e-06
iiwa_joint_7: -4.13576e-07
[ INFO] [1507912051.798420453, 33.419000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1507912051.815541847, 33.433000000]: Using planning interface 'OMPL'
[ INFO] [1507912051.847499878, 33.457000000]: Param 'default_workspace_bounds' was not set. Using default value: 10
[ INFO] [1507912051.848130614, 33.457000000]: Param 'start_state_max_bounds_error' was not set. Using default value: 0.05
[ INFO] [1507912051.850423179, 33.459000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1507912051.851076395, 33.460000000]: Param 'start_state_max_dt' was not set. Using default value: 0.5
[ INFO] [1507912051.851531947, 33.460000000]: Param 'jiggle_fraction' was not set. Using default value: 0.02
[ INFO] [1507912051.851947780, 33.461000000]: Param 'max_sampling_attempts' was not set. Using default value: 100
[ INFO] [1507912051.851990447, 33.461000000]: Using planning request adapter 'Add Time Parameterization'
[ INFO] [1507912051.852003430, 33.461000000]: Using planning request adapter 'Fix Workspace Bounds'
[ INFO] [1507912051.852014201, 33.461000000]: Using planning request adapter 'Fix Start State Bounds'
[ INFO] [1507912051.852024482, 33.461000000]: Using planning request adapter 'Fix Start State In Collision'
[ INFO] [1507912051.852040276, 33.461000000]: Using planning request adapter 'Fix Start State Path Constraints'
[ INFO] [1507912051.857888153, 33.467000000]: The timeout for planning must be positive (0.000000 specified). Assuming one second instead.
[ INFO] [1507912051.858403996, 33.467000000]: No planner specified. Using default.
[ INFO] [1507912051.860009828, 33.470000000]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1507912052.864713509, 34.346000000]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1507912052.864750805, 34.346000000]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1507912052.864765428, 34.346000000]: No solution found after 1.005971 seconds
[ INFO] [1507912052.885664352, 34.364000000]: Unable to solve the planning problem
[ERROR] [1507912052.885786886, 34.364000000]: Could not compute plan successfully
[ WARN] [1507912052.890851093, 34.368000000]: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded.
zweistein commented 7 years ago

Hi,

I would like to ask to create issues that are highly usable for other partners too. This one issue has already several solved problems/issues. I'm going to close this track, so please create a new issue with a descriptive title/category.

Could you please give more detail about the demo that you are running? Which demo is this exactly? Please refer to the README and the tutorial video. Is this problem only showing up on the new install or was this also present on our setup?

The errors are consistently pointing on a missing TF frame. So, please check which frame is missing and which are present.

$ rosrun ec_grasp_planner planner.py --grasp WallGrasp --object_frame object --ros_service_call --rviz --handarm RBOHand2Kuka

Why did you add --object_frame object? I suspect this broke the pipeline.