Dear Matei,<br><br>Thanks once more for your very complete answer. I now sorted my lifting problem (some stupid bug in my code). <br><br>I'm still having issues with the planning environment. I'm using a Kinect as a source of 3D data. I have to convert the pcl2 to a pcl using point_cloud_converter to be able to build the collision map  (Radu told me I was mad to use that as it's not efficient at all, but I'm waiting for the collision map to use a pcl2 :) ). For the time being I'm just returning true in isStateValid. I haven't had time to debug that yet (I'll try to work on it tomorrow), but I'm sending this just in case you have any miraculous hindsight. <br>
<br>I'm not sure on which topic I should subscribe to visualize the attached_object in rviz as well. (I looked at the tf but couldn't see any tf moving with the model, once the hand has grasped the can).<br><br>Cheers,<br>
<br>Ugo<br><br><br><br><div class="gmail_quote">On Mon, Feb 7, 2011 at 4:53 PM, Matei Ciocarlie <span dir="ltr"><<a href="mailto:matei@willowgarage.com">matei@willowgarage.com</a>></span> wrote:<br><blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex;">
Hi Ugo,<br>
<br>
Glad to hear things are working out. Writing a /hand_posture_execution<br>
for a new hand is expected behavior, it's what we envisioned people<br>
doing for porting the code to a new robot.<br>
<br>
As for the lift, maybe this will help. The reason for the lift is so<br>
that, when the grasp is complete, the robot is in a state that the<br>
environment servers does not consider in collision; otherwise, the<br>
motion planners will refuse to take the arm out of there. The amount<br>
of lift is specified in the PickupGoal as a GripperTranslation. Much<br>
like the approach, there are two values: one is how much you'd like<br>
the object to be lifted, and the other one is the minimum lift<br>
distance that should be deemed feasible before the grasp is even<br>
attempted.<br>
<br>
Now, for the direction of the lift, note that the lift component of<br>
the PickupGoal specifies a direction as a Vector3Stamped. In theory,<br>
this one can be used to specify a direction in any coordinate frame<br>
that tf knows about. However, there is a catch: due to the way the<br>
internals of our system work, the grasping pipeline can only use lift<br>
directions specified in either the hand's coordinate system or the<br>
robot's coordinate system. These are the "robot_frame" and<br>
"hand_frame" specified in your .yaml hand configuration file.<br>
<br>
In practice, this is enough to cover most use cases. We've found two<br>
main use cases<br>
- when getting something from a table, you want to lift "up" (in the<br>
opposite direction to gravity). For this case, use the "up" direction<br>
in your robot frame.<br>
- when getting something from a shelf, you want to lift in the<br>
opposite of the gripper approach direction. For this case, use the<br>
opposite of the approach direction in your hand frame.<br>
<br>
Hope this helps; let us know if you run into more problems.<br>
<br>
Best,<br>
<font color="#888888">Matei<br>
</font><div><div></div><div class="h5"><br>
On Mon, Feb 7, 2011 at 7:24 AM, Ugo Cupcic <<a href="mailto:ugo@shadowrobot.com">ugo@shadowrobot.com</a>> wrote:<br>
> Hi,<br>
><br>
> Thanks a lot for both your answers. It made things a bit clearer.<br>
><br>
> I'm using the grasping pipeline, but I had to modify some nodes because I'm<br>
> adapting it to our robot hand / arm + using the kinect as a 3d sensor.<br>
> I had to write my own move_arm_simple_action / hand_posture_execution nodes.<br>
><br>
> Modifying the hand_approach_direction in my hand_description config file<br>
> solved the problem of the "backward" approach.<br>
><br>
> I still have to get the lift action to behave, but we're getting there :)<br>
><br>
> Cheers,<br>
><br>
> Ugo<br>
><br>
><br>
> On Fri, Feb 4, 2011 at 1:07 AM, Matei Ciocarlie <<a href="mailto:matei@willowgarage.com">matei@willowgarage.com</a>><br>
> wrote:<br>
>><br>
>> Hi Ugo,<br>
>><br>
>> Are you using the grasping pipeline by calling the Pickup action? If so,<br>
>> the grasping pipeline will do for you all of what Kaijen explained. Here are<br>
>> a few notes:<br>
>><br>
>> - the pre-grasp stored in the database is not used. Initially we though it<br>
>> would be, but then we realized that how far back the pre-grasp should be<br>
>> from the grasp is often application-specific.<br>
>><br>
>> - the Pickup action goal thus specifies how far back the pre-grasp should<br>
>> be. There are two relevant fields:<br>
>><br>
>> float32 desired_approach_distance<br>
>> float32 min_approach_distance<br>
>><br>
>> - the grasping pipeline will attempt to find a pre-grasp that is "behind"<br>
>> the grasp by at least min_approach_distance, and ideally<br>
>> desired_approach_distance. This pre-grasp must be reachable and out of<br>
>> collision. It will do this by starting at the grasp position, then doing<br>
>> interpolated IK "backwards" as far as possible, up to<br>
>> desired_approach_distance. This explains why Interpolated IK markers show up<br>
>> the at the grasp location first, then move back.<br>
>><br>
>> - what does "backwards" mean? That is specified on the parameter server.<br>
>> The grasping pipeline will look for a set of parameters telling it, among<br>
>> others:<br>
>>  * what is the tf name of reference frame of the hand - this is the frame<br>
>> that the grasp pose refers to<br>
>>  * what is the "approach" direction, relative to the hand frame, when<br>
>> executing a grasp. A pre-grasp will be computed by translating a grasp in<br>
>> the opposite of the approach direction.<br>
>><br>
>> For the PR2, this is specified in<br>
>> pr2_object_manipulation_launch/config/pr2_hand_descriptions.yaml. You will<br>
>> probably need to adapt this file to your hardware.<br>
>><br>
>> Let us know if you have more problems, we'd be glad to help. Also, if you<br>
>> could include some rviz screenshots from multiple viewpoints that would<br>
>> help, as it's hard to tell from one image what things look like in 3D.<br>
>><br>
>> Best,<br>
>> Matei<br>
>><br>
>> On Thu, Feb 3, 2011 at 1:06 PM, Kaijen Hsiao <<a href="mailto:kaijenhsiao@gmail.com">kaijenhsiao@gmail.com</a>><br>
>> wrote:<br>
>>><br>
>>> Hi Ugo,<br>
>>> I'm not sure exactly which stack/code you are using, but those markers<br>
>>> look like they're coming from the interpolated IK motion planner.  The<br>
>>> interpolated IK motion planner can search for a consistent/feasible<br>
>>> path starting from either end--if the start_from_end parameter is<br>
>>> true, then the planner finds IK solutions for the end pose first, then<br>
>>> works its way backwards towards the start pose, using the last set of<br>
>>> found arm angles as the starting point.<br>
>>><br>
>>> Typically when you're planning from pregrasp to grasp, you want to<br>
>>> enter pregrasp as start and grasp as end so the trajectory times are<br>
>>> correct, but you want to ask it to start searching from the grasp<br>
>>> (end) pose and work its way backwards.  That's because you typically<br>
>>> can't compromise on the final grasp pose, but you might not mind if<br>
>>> the Cartesian path backwards towards the pregrasp gets almost but not<br>
>>> all the way there.  That's what the error codes tell you--how far it<br>
>>> got.  (You can also set the steps_before_abort param to 0 if you just<br>
>>> want it to abort if it finds any infeasible/inconsistent poses at<br>
>>> all.)<br>
>>> If you get an incomplete path back, you can decide whether you're<br>
>>> willing to just motion plan to a slightly closer pregrasp pose, or if<br>
>>> you want to abort.  If the markers don't make it all the way to the<br>
>>> pregrasp, it means that the path is incomplete--the next step towards<br>
>>> the pregrasp either had no collision-free IK solution, or else had no<br>
>>> solution consistent with the last step (within consistent_angle for<br>
>>> all joint angles from the last step).<br>
>>><br>
>>> I'm not sure what you're running to get to the pregrasp, but you<br>
>>> probably want to use move_arm or some such to move to the start of the<br>
>>> (completed portion of the) trajectory returned by the interpolated IK<br>
>>> motion planner, then use a normal joint trajectory movement to get<br>
>>> through the rest of the trajectory.  (Or you can just use a Cartesian<br>
>>> controller, assured that there actually is a feasible/consistent path<br>
>>> to actually reach the grasp pose from that set of joint angles.)<br>
>>><br>
>>> Hope that helps.<br>
>>><br>
>>> -Kaijen<br>
>>><br>
>>><br>
>>> On Thu, Feb 3, 2011 at 8:25 AM, Ugo Cupcic <<a href="mailto:ugo@shadowrobot.com">ugo@shadowrobot.com</a>> wrote:<br>
>>> > Hi all,<br>
>>> ><br>
>>> > I'm still trying to get the manipulation stack to work with our arm and<br>
>>> > hand<br>
>>> > and I ran into some problem:<br>
>>> ><br>
>>> > As you can see here, A path from pregrasp to grasp is found:<br>
>>> > <a href="http://img267.imageshack.us/img267/199/rviz013.png" target="_blank">http://img267.imageshack.us/img267/199/rviz013.png</a><br>
>>> ><br>
>>> > But:<br>
>>> >  > the path seems to be the wrong way around: the first markers are at<br>
>>> > the<br>
>>> > bottom, instead of at the top.<br>
>>> >  > when moving the arm, the arm moves maybe 5cm below the markers (on<br>
>>> > the<br>
>>> > screenshot, it's at position 0 which should be pregrasp).<br>
>>> ><br>
>>> > The values I put in the database are:<br>
>>> > grasp_pregrasp_position =<br>
>>> > "{0,0,0.281533,0.707106781186548,0.707106781186548,0,0}"<br>
>>> > grasp_grasp_position =<br>
>>> > "{0,0,0.213018,0.707106781186548,0.707106781186548,0,0}"<br>
>>> ><br>
>>> > Any help greatly appreciated.<br>
>>> ><br>
>>> > Cheers,<br>
>>> ><br>
>>> > Ugo<br>
>>> ><br>
>>> > --<br>
>>> > Ugo Cupcic | Shadow Robot Company | <a href="mailto:ugo@shadowrobot.com">ugo@shadowrobot.com</a><br>
>>> > Software Engineer | 251 Liverpool Road |<br>
>>> > need a Hand? | London N1 1LX | +44 20 7700 2487<br>
>>> > <a href="http://www.shadowrobot.com/hand/" target="_blank">http://www.shadowrobot.com/hand/</a> @shadowrobot<br>
>>> ><br>
>>> ><br>
>>> > _______________________________________________<br>
>>> > ros-users mailing list<br>
>>> > <a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
>>> > <a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
>>> ><br>
>>> ><br>
>>> _______________________________________________<br>
>>> ros-users mailing list<br>
>>> <a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
>>> <a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
>><br>
>><br>
>><br>
>> --<br>
>> Matei Ciocarlie<br>
>> Research Scientist<br>
>> Willow Garage Inc.<br>
>> 650-475-9780<br>
>><br>
>> _______________________________________________<br>
>> ros-users mailing list<br>
>> <a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
>> <a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
>><br>
><br>
><br>
><br>
> --<br>
> Ugo Cupcic | Shadow Robot Company | <a href="mailto:ugo@shadowrobot.com">ugo@shadowrobot.com</a><br>
> Software Engineer | 251 Liverpool Road |<br>
> need a Hand? | London N1 1LX | +44 20 7700 2487<br>
> <a href="http://www.shadowrobot.com/hand/" target="_blank">http://www.shadowrobot.com/hand/</a> @shadowrobot<br>
><br>
><br>
> _______________________________________________<br>
> ros-users mailing list<br>
> <a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
> <a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
><br>
><br>
<br>
<br>
<br>
--<br>
Matei Ciocarlie<br>
Research Scientist<br>
Willow Garage Inc.<br>
650-475-9780<br>
_______________________________________________<br>
ros-users mailing list<br>
<a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
<a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
</div></div></blockquote></div><br><br clear="all"><br>-- <br>Ugo Cupcic | Shadow Robot Company | <a href="mailto:ugo@shadowrobot.com" target="_blank">ugo@shadowrobot.com</a> <br>Software Engineer | 251 Liverpool Road | <br>
need a Hand? | London N1 1LX | +44 20 7700 2487 <br><a href="http://www.shadowrobot.com/hand/" target="_blank">http://www.shadowrobot.com/hand/</a> @shadowrobot <br><br>