"loadRobotGraspData" code in "simple pick place"

[ltaoist]

As infer in issues https://github.com/davetcoleman/clam/issues/6 ...

The clam_grasp_data.yaml file may not be right, it need some fix ...

[davetcoleman]

I will merge and test, then maybe fix.

[davetcoleman]

Thanks!

[rojas70]

Dave, thanks for the code updates.

Here is an update on what I am doing.

Trying to get block_pick_place.cpp to work.

• I have created a roslaunch file for it in which I use rosparam to load parameters according to moveit_simple_grasps/moveit_blocks.h :

planning_group_name: 'arm'
ee_group_name: 'gripper_group'

At first I could not read these parameters, I had to remove the initialization of nh("~") to a private name. I am still not so familiar with these differences, but it worked well when I removed it:

// did not initialize nh to nh("~")
nh_.getParam("ee_group_name", ee_group_name_);
nh_.getParam("planning_group_name", planning_group_name_);

When I roslaunched again, it said I had not added base_link to the param server. So I added it. The error screen is shown below for reference:

>>$ roslaunch clam_pick_place block_pick_place.launch 
... logging to /home/vmrguser/.ros/log/2c3ce85a-f885-11e3-bade-c48508b13bfe/roslaunch-vmrguser-XPS-L521X-15851.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://vmrguser-XPS-L521X:34841/

SUMMARY
========

PARAMETERS
 * /ee_group_name
 * /planning_group_name
 * /rosdistro
 * /rosversion

NODES
  /
    block_pick_place_vmrguser_XPS_L521X_15851_8153052393670975996 (clam_pick_place/block_pick_place)

auto-starting new master
process[master]: started with pid [15865]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 2c3ce85a-f885-11e3-bade-c48508b13bfe
process[rosout-1]: started with pid [15878]
started core service [/rosout]
process[block_pick_place_vmrguser_XPS_L521X_15851_8153052393670975996-2]: started with pid [15890]
[ INFO] [1403273661.943514138]: Starting Clam Block Pick Place
[ INFO] [1403273661.950291290]: Starting MoveIt Blocks
[ INFO] [1403273661.954760898]: End Effector: gripper_group
[ INFO] [1403273661.954841119]: Planning Group: arm
[FATAL] [1403273661.958080022]: Unable to construct robot model. Please make sure all needed information is on the parameter server.
terminate called after throwing an instance of 'std::runtime_error'
  what():  Unable to construct robot model. Please make sure all needed information is on the parameter server.

After getting the base_link param in moveit_simple_grasps/moveit_blocks.h, I got a couple of errors related to not being able to read kinematics.yaml files. I saw that this yaml file was already loaded in moveit_rviz, but for some reason is not being read properly. So I added it to my current launch file and fixed it. Error screen below for reference:

[ERROR] [1403277250.570284338]: The ClamArm gripper controller expects a joint trajectory with one point only, but 2 provided)
[ERROR] [1403277250.570393245]: Failed to send trajectory part 1 of 1 to controller clam_gripper_controller

And in the block_pick_place screen I got:

PARAMETERS
 * /base_link
 * /ee_group_name
 * /gripper_group/end_effector_name
 * /gripper_group/end_effector_parent_link
 * /gripper_group/grasp_pose_to_eef
 * /gripper_group/grasp_pose_to_eef_rotation
 * /gripper_group/grasp_posture
 * /gripper_group/grasp_time_from_start
 * /gripper_group/joints
 * /gripper_group/pregrasp_posture
 * /gripper_group/pregrasp_time_from_start
 * /planning_group_name
 * /rosdistro
 * /rosversion
....
[ERROR] [1403277229.927047278]: No active end effectors found! Make sure kinematics.yaml is loaded in this node's namespace!
....
[ INFO] [1403277231.936420337]: Picking 'Block1'
[ INFO] [1403277232.441788294]: Generated 34 grasps.
[ INFO] [1403277232.444346810]: Generated 68 grasps.
[ INFO] [1403277232.446469275]: Generated 102 grasps.
[ INFO] [1403277232.449685822]: Generated 136 grasps.
....
[ERROR] [1403277245.981903268]: Unable to publish EE marker because marker has not been loaded yet

This is the final stage and might need some help here, the grasps were generated, but the controller failed during execution:

[ INFO] [1403278583.627902825]: Starting MoveIt Blocks
[ INFO] [1403278583.631678341]: End Effector: gripper_group
[ INFO] [1403278583.631738675]: Planning Group: arm
[New Thread 0x7fffe1d68700 (LWP 30871)]
[ INFO] [1403278583.641497136]: Loading robot model 'clam'...
[tcsetpgrp failed in terminal_inferior: Inappropriate ioctl for device]
[tcsetpgrp failed in terminal_inferior: Inappropriate ioctl for device]
[ INFO] [1403278584.158086914]: shoulder_pan_joint -2.61799 1.98395 1
[ INFO] [1403278584.158191390]: shoulder_pitch_joint -1.19963 1.89994 1
[ INFO] [1403278584.158247872]: elbow_roll_joint -2.61799 0.705631 1
[ INFO] [1403278584.158295425]: elbow_pitch_joint -1.59534 1.93282 1
[ INFO] [1403278584.158374837]: wrist_roll_joint -2.61799 2.61288 1
[ INFO] [1403278584.158432576]: wrist_pitch_joint -1.59534 1.98395 1
[ INFO] [1403278584.158483470]: gripper_roll_joint -2.61799 2.61288 1
[ INFO] [1403278584.909637469]: Ready to take MoveGroup commands for group arm.
[ INFO] [1403278584.935804073]: Loading robot model 'clam'...
[ INFO] [1403278585.375840937]: shoulder_pan_joint -2.61799 1.98395 1
[ INFO] [1403278585.375901969]: shoulder_pitch_joint -1.19963 1.89994 1
[ INFO] [1403278585.375926325]: elbow_roll_joint -2.61799 0.705631 1
[ INFO] [1403278585.375950989]: elbow_pitch_joint -1.59534 1.93282 1
[ INFO] [1403278585.375972176]: wrist_roll_joint -2.61799 2.61288 1
[ INFO] [1403278585.376015891]: wrist_pitch_joint -1.59534 1.98395 1
[ INFO] [1403278585.376037331]: gripper_roll_joint -2.61799 2.61288 1
[New Thread 0x7fffe11fc700 (LWP 31152)]
[New Thread 0x7fffe09fb700 (LWP 31153)]
[Thread 0x7fffe09fb700 (LWP 31153) exited]
[ INFO] [1403278588.952073682]: Picking 'Block1'
[ INFO] [1403278589.457094855]: Generated 34 grasps.
[ INFO] [1403278589.459790492]: Generated 68 grasps.
[ INFO] [1403278589.461490288]: Generated 102 grasps.
[ INFO] [1403278589.464417083]: Generated 136 grasps.
[ WARN] [1403278608.158684650]: Fail: ABORTED: Solution found but controller failed during execution
[ERROR] [1403278608.159156833]: Pick failed.
[ INFO] [1403278608.159193266]: Retry? (y/n)

The video can be seen here: http://youtu.be/53WBq4WVcck

It seems its's failing at: return movegroup->pick(block_name, possible_grasps)

[davetcoleman]

I already created a roslaunch file for this executable, did you not see it? https://github.com/davetcoleman/clam/blob/master/clam_pick_place/launch/pick_place.launch#L16

(as documented on the ClamArm website)