search

ros-aldebaran / romeo_robot

ROS stack for Aldebaran's Romeo robot
4 stars 7 forks source link

Position of Hands and grippers #10

Open lluissalord opened 8 years ago

lluissalord commented 8 years ago

I have take a look on the package romeo_description, which has de model of Romeo. And the position of the Hands and so the gripper is between the wrist and the elbow. Correct me if I'm wrong but I think that may be should be where the hand grasp an object.

For what I have measured on the real Romeo, I think that should be approximately at x = 0.10 (m) y = 0.0 (m) and z = - 0.03 (m) relatives to the wrist cords which is the parent link of the Hand.

And thank you for all the good work you have done

nlyubova commented 8 years ago

Right, the gripper's position is like this. For grasping, you can use the RHand and LHand frame as grasping frame (like here https://github.com/davetcoleman/moveit_simple_grasps/blob/indigo-devel/config/romeo_grasp_data.yaml ), since their position is perfect for grasping (and it worked well for me with Moveit). Grippers are used only as revolute joints for closing and opening hands (maybe one day we change their position if it is really needed).

nlyubova commented 8 years ago

@lluissalord how do you grasp with romeo? with moveit or no?

lluissalord commented 8 years ago

For now I haven't grasp anything yet. I only have made some test of using moveit. Using the romeo_dcm and the launcher demo_real.launch from romeo_moveit_config.

Then with the MoveGroup class I make the planning and movement of the chain arm_hand_left or arm_hand_right (well how I have code that I can chose between all the chains if I want) to the desired position and then it's suppose to grasp. So using this chain it has the end effector with the gripper which is in the same position of the RHand or LHand, so I just made the cheat of put my cord to the RHand and LHand in the romeo_arms.xacro from the romeo_description and it worked well, but I know that it's not the proper way so I asked here about it.

nlyubova commented 8 years ago

great! The chain arm_hand_right is the old one. You can try "left_arm" and "right_arm" with attached end-effectors (https://github.com/ros-aldebaran/romeo_moveit_config/blob/master/config/romeoH37.srdf#L73), this is the one we use. Did you try it?

There is also Moveit_simple_grasps configured for these chains.

lluissalord commented 8 years ago

Then if I understand you are using a chain which the end effector is the Wrist and then you configure the grasping action to have an offset?

Now I will test the moveit_simple_grasps in simulation and may be this afternoon or tomorrow I will test on the real one.

mikaelarguedas commented 8 years ago

Indeed the gripper frame position wrong, it seems to be placed at the position of the motor used to close the hand and not the point where the hand actually grasps an object as it should be, cf. REP120. @nlyubova Are you using an offset when you perform grasping? If yes how has it been determined ? I would like to find a way to fix the URDF model without putting measured/inaccurate values. Is there any way to get a precise value of that offset? It may already exist in alrobotmodel ?

nlyubova commented 8 years ago

@mikaelarguedas I am using RHand and LHand frame (see the link in the previous comment) and its position suits well for that perpose

nlyubova commented 8 years ago

or we could take the end of the link (but not in the middle as it is now)

lluissalord commented 8 years ago

I think that the offset that @mikaelarguedas is talking about may be can be the one of this config file https://github.com/nlyubova/moveit_simple_grasps/blob/romeo-dev/config/romeo_grasp_data.yaml. Here it seems like an offset from the wrist to grasp objects. So may be one solution could be using that offset to make the model with the end of the link here.

mikaelarguedas commented 8 years ago

@nlyubova RHand and LHand are not frames but joints in the urdf, they are the joints you command to close the hand. According to the link in your previous comment, the end effector used is L/RWristYaw_link (https://github.com/ros-aldebaran/romeo_moveit_config/blob/master/config/romeoH37.srdf#L73). From the link of the first comment https://github.com/davetcoleman/moveit_simple_grasps/blob/indigo-devel/config/romeo_grasp_data.yaml#L30 We can see that an offset of [-0.113, 0.036, 0.036] is added to the LWristYaw_link position. According to all the comments in the file it seems that this value has been find empirically after many attempts and tuning. This is why I wanted to know if we could have an accurate value from Aldebaran (robotmodel or mechanical data). This would avoid people to have to tune this offset by hand and use directly a l/r_gripper frame for their end effector (as defined in REP120)

mikaelarguedas commented 8 years ago

@lluissalord Exactly, if we cannot have an official value we could use the offsets tuned by davetcoleman to offset the gripper frames from the wrist in the URDF model.

lluissalord commented 8 years ago

@mikaelarguedas for the commits I have seen it seems it was tuned by @nlyubova so may be she can explain how she found this offsets

mikaelarguedas commented 8 years ago

Perfect! Thanks for pointing it out @lluissalord . That would be a really useful information that other users can refer to. @nlyubova if you agree to give us more detail about the process to tune those and how reliable they are I can integrate them in the next version of the URDF.

nlyubova commented 8 years ago

@lluissalord and @mikaelarguedas, the offsets that I've added to the config https://github.com/nlyubova/moveit_simple_grasps/blob/romeo-dev/config/romeo_grasp_data.yaml correspond to the distance from end-effectors and teh end-effectors are attached to LWristYaw_link and RWristYaw_link, as in the config and SRDF. I've set these config params while grasping with moveit simulator and also the real robot. Not sure, if it is needed to integrate these values to URDF since they allow different grasping configurations (from side and from top of object). If we integrate one of them to URDF, (better from side), then we need to recompute for the top and change this config file ...... Is there any point to do it? It is much better to keep the URDF as it is since it matches the one in Naoqi (and anyway it is regenerated from there).

So just use these config file (or the one more detailed from my forked repo) and please let me know, if you find other stable poses

mikaelarguedas commented 8 years ago

@nlyubova I agree that the current configuration works and you can successfully grasp objects with it (thumbs up! by the way). As you said it is not worth it to do all this extra work just to comply with the standard.

The URDF files are indeed generated from Naoqi, but the gripper frames (not initially provided by Naoqi) are added to the URDF at generation time. I am then offering to fix the URDF generation given that these frames are not used by anybody and their current position is misleading from the user's perspective. There are 2 options to do so:

nlyubova commented 8 years ago

ah if these frames are not re-generated by Naoqi, then let's keep them and change their positions to the position of LWristYaw_link+the offset for grasping by object-side. It should allow to grasp without grasp generator but simply with Moveit Action button. An I will add a new end-effector to SRDF or a new move group ending by this frame !

mikaelarguedas commented 8 years ago

Exactly, that is what I hoped we can do. Will you have access to a Romeo in the near future to confirm that the changes to URDF/SRDF don't break anything on your grasping tasks?

nlyubova commented 8 years ago

sure, I'm sitting all days long in front of it :)

mikaelarguedas commented 8 years ago

Awesome! I'll let you know when it's ready for testing (certainly later this week). Another question to be sure, you tuned these offset for both pepper, nao and romeo right ? I can use the offset from the config files in your repo to update all robots' URDF? For NAO, on which version of Nao did you make your measurements? a V50?

nlyubova commented 8 years ago

yes, for all of them, by the way, moveit_simple_grasps will not work like it is with our robots, but it works with my fork https://github.com/nlyubova/moveit_simple_grasps/tree/indigo-devel/config please take the offsets from there but please be careful because I use the left hand for grasping from side and the right hand for grasping from top :) However, it is better to keep them symmetrical in URDF //left

side grasp -pi/3 - best

grasp_pose_to_eef : [-0.113, 0.036, 0.036]
grasp_pose_to_eef_rotation : [-1.032, 0.0, 0.0]

//right

side grasp pi/3 - best

#grasp_pose_to_eef : [-0.113, -0.036, 0.036]
#grasp_pose_to_eef_rotation : [1.032, 0.0, 0.0]
lluissalord commented 8 years ago

With the purpose of not losing work done before for @nlyubova and have the option of doing a good grasp by top may be can be useful to have a config file that configure dynamically the offset depending on if you want to make a grasp by side or by top. So if you want to make by side is the default config and if you want by top then load that romeo_grasp_data_top.yaml. It's only an idea to can use both ways of grasps with both arms and having the gripper in the proper position

lluissalord commented 8 years ago

@nlyubova about your fork, the romeo branch is the most updated no?

nlyubova commented 8 years ago

@mikaelarguedas please take in mind that we need the side-grasp and not the top-grasp (the top-grasp is only for temporal/test usage)

nlyubova commented 8 years ago

@lluissalord yes there are few changes in computing hand orientation based on several axes

mikaelarguedas commented 8 years ago

@nlyubova ok I'll make sure to use the side ones for all of them. I would reiterate on @lluissalord question: I should use the values in the config files of the romeo-dev branch given that they have the most recen values right ? @lluissalord I agree with that. I think the first step will be to keep a copy of all current config files and try only the grasping_from_side with the new frame. Then we'll create another config file that will be loaded if grasping from top is wanted. Sounds good to me

nlyubova commented 8 years ago

yes romeo-dev (sorry forgot to mention)

On Mon, Apr 11, 2016 at 11:06 AM, Mikael Arguedas notifications@github.com wrote:

@nlyubova https://github.com/nlyubova ok I'll make sure to use the side ones for all of them. I would reiterate on @lluissalord https://github.com/lluissalord question: I should use the values in the config files of the romeo-dev branch given that they have the most recen values right ? @lluissalord https://github.com/lluissalord I agree with that. I think the first step will be to keep a copy of all current config files and try only the grasping_from_side with the new frame. Then we'll create another config file that will be loaded if grasping from top is wanted. Sounds good to me

— You are receiving this because you were mentioned. Reply to this email directly or view it on GitHub https://github.com/ros-aldebaran/romeo_robot/issues/10#issuecomment-208242177

Natalia Lyubova, PhD Scientist, R&D Aldebaran Robotics www.aldebaran.com +33 6 50 51 74 11

nlyubova commented 8 years ago

by the way, please be careful that the sign for opening and closing hand with our robots is inverse from the sign of MoveIt so when the hand is open on a robot it is close in MoveIt

mikaelarguedas commented 8 years ago

that should not be modified by the changes I intend to make. But yeah that's a fair point, I remember that gave us a hard time so time ago...

mikaelarguedas commented 8 years ago

Just realized that the actual end of the kinematic chain on Romeo arm is LWristRoll_link (also called l_wrist in the URDF files) and not LWristYaw_link. That would explain why the joint LWristPitch is considered as part of the end effector in here and not as part of the arm. It seems that this inconsistency occurs only for Romeo; Nao and Pepper are using l_wrist as the end of the arm frame. @nlyubova Was that decision made on purpose ? To allow to have different configurations for side and top grasp maybe?

So I guess I will not update the URDF for Romeo because it will not respect the standard (l/r_wrist end of the arm and l/r_gripper grasping position according to gripper attached to the wrist). And if we define it as expected by ROS, the current working grasps will very likely not work anymore.

What do you think about it ? Should we leave everything as is to avoid extra work? Or should we try to make Romeo match the other humanoids/standard?

nlyubova commented 8 years ago

@mikaelarguedas no worries about the gripper, we will probably update its position later on after testing. The configuration of romeo's arm +hand as an end-effector is the only acceptable by Moveit at that time when I made it (also possible that we can do better now). Since some of planners need an arm of 6DOF (and this is the reason of difference between Nao, Pepper and Romeo, since Romeo have one more DOF for its arm) . and also it is good for us to have different grasping configurations.

@lluissalord if you will have a great idea how to improve the moveit config for Romeo after your tests, let us know please

mikaelarguedas commented 8 years ago

@nlyubova I see, surprising that moveit couldn't handle the 7DOF of romeo's arm at the time, I thought it was working for PR2. In conclusion I'll keep the issue on URDF generation https://github.com/ros-naoqi/naoqi_bridge/issues/55 open and won't submit any update until the day it will actually make sense to change it. Keep up the good work and please share videos when you have some! Always a pleasure to see Romeo do cool stuff.

nlyubova commented 8 years ago

@mikaelarguedas it is not the problem of MoveIt but some of planners. If you can make a better configuration, we are always happy to test it!

lluissalord commented 8 years ago

I've been thinking that may be could be a good idea to use IKFast, for what I have read with that you can have a very good performance and fastly. Because I have seen that to make a move of the arms I need a tolerance of 0.10 or 0.20, lasting a lot of time and I think that is not the best way of working.

mikaelarguedas commented 8 years ago

Yeah that's definitely worth a shot. It could also be an interesting option for other Aldebaran robots given that IKFast handles nicely approximate solutions so it could give much better results for less than 6DOF arms robots

lluissalord commented 8 years ago

I have tried to make the .cpp of IKFast but or my computer is not good enough and I don't reach the solutions. In the tutorials I found it seems easy and that doesn't take a lot of time, but it seems that is not my case. Till this week I didn't know anything about IKFast so probably I'm doing wrong. If some of you thinks that could get it and want to do it I will thank you because I don't know what more I can try.

mikaelarguedas commented 8 years ago

I don't think I would have time to try it in the near future but I would suggest that you try running IKFast on one of the "example/simple robots" to see if everything compiles and run correctly before trying to find solutions for a complicated robot like Romeo.

nlyubova commented 8 years ago

@lluissalord I'm interested in IKfast too but do not have time for it at the moment (will try to work on it in the following months). Please, Let us if you get any progress earlier.

lluissalord commented 8 years ago

I have try more to get it but for now I have failed. I posted the problems I found and the way how to reproduce the problems in this link. I will wait if anyone gives me any tipp to follow and may be post it in the MoveIt group too.

@mikaelarguedas I have tried some examples and it worked. So the problem is something about Romeo.

mikaelarguedas commented 8 years ago

Thanks for the update. It's good news that it works on other examples, it means that your setup is fine and the problem is indeed related to the romeo model.

It's true that rounding the numbers to 5 or less decimal is usually necessary to allow IKfast to converge without exploding. (the script round_collada_numbers.py should also be available in the moveit_ikfast ros package)

I tried today and could successfully generate code/create a plugin for the left arm. I ran the script on an urdf/collada file with all values rounded to 5 decimals. My URDF was not a full romeo, I commented out lines 12-14 from the xacro file. It took 34min to complete.

I pasted below the commands I used to both install and generate the files. Hopefully it will point out the differences between our setups and help identify the problem. I'm discovering IKFast as much as you do so the list of commands below will very likely lead you to a not fully working solution, I unfortunately neither have a robot nor much time to do testing in Moveit. I hope this will help move forward and please let us know how it went so that other users can refer to it.

Good Luck!

Installation:

Create IKFast plugin:

time python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=romeo.dae --iktype=transform6d --baselink=2 --eelink=8 --freeindex=8 --savefile=/media/ROS/ros/aldeb_ws/src/romeo_robot/romeo_description/urdf/romeo_generated_urdf/romeo_ikfast_left_arm.cpp

As you can see I arbitrarily chose the last link to be the free link of the chain. That may not be the best choice for manipulation purposes.

               solver_version = int(line_search.group(1), 16)
lluissalord commented 8 years ago

@mikaelarguedas Thank you so much! I have tried to reinstall all the thinks following your steps. I had to change a few things like:

After installing all, I have created like you said the new romeo.dae and I have used your command to have the IKfast .cpp but it haven't worked. Now last 13min to give the error and it seems is in another part. So now is doing something different, but still not working. Reading the output in the console it stops after trying to symplify a determinant but it get an error of too many depth recursion.

While I'm writing this is trying to solve the same with --eelink=9 --freeindex=6. If is not working I will continue trying differents things. But if I get desesperated, could I ask you for doing the IKFast .cpp for the left and right arm and then upload here?

lluissalord commented 8 years ago

The test with --eelink=9 --freeindex=6 worked. But with this I suppose that I would get the pose of the l_gripper that like we said in this topic is not well positioned. So now I'm doing other tests of combinations of base_link, eelink and freeindex and see if I can have one useful.

mikaelarguedas commented 8 years ago

Glad to see that we're making progress !

Regarding installation For Openrave compilation, I also needed to explicitely state which namespace the shared_ptr belonged to, if you want fclrave to build you need to replace shared_ptr by std::shared_ptr in fclspace.h. For bullet I ran the following commands:

sudo add-apt-repository ppa:joern-schoenyan/libbullet280
sudo apt-get update
sudo apt-get install libbullet2.80
sudo apt-get install libbullet-dev=2.80*

And I just realize that I installed ode from bitbucket archive not sourceforge:

wget https://bitbucket.org/odedevs/ode/downloads/ode-0.13.tar.gz
tar -zxvf ode-0.13.tar.gz
cd ode-0.13/
./configure --enable-double-precision --enable-shared
make
sudo make install

Anyway it shouldn't make much of a difference.

IKFast How weird that the same command with --baselink=2 --eelink=8 --freeindex=8 failed for you. It seems that we now have the same setup and I assume we have the same URDF/xacro files? Did you get romeo_description from sources or from packages ?

It seems that the only thing I changed in my URDF files compard to master is that I adjusted the position of the grippers to be somewhere meaningful based on this discussion

--- a/romeo_description/urdf/romeo_generated_urdf/romeo_arms.xacro
+++ b/romeo_description/urdf/romeo_generated_urdf/romeo_arms.xacro
@@ -109,7 +109,7 @@
        <joint name="LHand" type="revolute">
                <parent link="l_wrist"/>
                <child link="l_gripper"/>
-               <origin rpy="1.5708 0.0450146 -3.14159" xyz="-0.0579412 -0.0159 
+               <origin rpy="1.032 0 0" xyz="0.113 -0.036 -0.03"/>
                <axis xyz="1.0 0 0"/>
                <limit effort="0.649997" lower="0" upper="1.0" velocity="59.8925
        </joint>
@@ -225,7 +225,7 @@
        <joint name="RHand" type="revolute">
                <parent link="r_wrist"/>
                <child link="r_gripper"/>
-               <origin rpy="1.5708 0.0450146 -3.14159" xyz="-0.0579412 -0.0159 
+               <origin rpy="-1.032 0 0" xyz="0.113 0.036 -0.03"/>
                <axis xyz="1.0 0 0"/>
                <limit effort="0.649997" lower="0" upper="1.0" velocity="59.8925
        </joint>

You may want to apply the same change if you can generate IK files with l/r_gripper as the end effector.

I'll definitely try to generate several IK files for different limbs and configuration and let you know what I find out/post the files somewhere. We may want to regenerate those once the romeo model has been updated (it seems that some values are not up to date)

lluissalord commented 8 years ago

I changed the l/r_gripper to `xyz="0 0 0" and I could do the IK file of the left hand from LShoulder to l_gripper, but from RShoulder to r_gripper it gave me the same error has other combination.

Now I have tried to reinstall with as you said in the last post (I didn't use double precision and may be that change something), but still have the problem with fclrave. I add #include <memory.h> to the fclspace.h and I look for every shared_ptr and change them for std::shared_ptr. In the file fclcollision.h I haven't found any. I have upload the files here (with extension .txt). The error that gives me is:

/openrave-master/plugins/fclrave/fclrave.cpp:18:
/home/lluis/IKFast_libraries/openrave-master/plugins/fclrave/fclspace.h:11:9: error: ‘shared_ptr’ in namespace ‘std’ does not name a type
 typedef std::shared_ptr<fcl::CollisionGeometry> CollisionGeometryPtr;

fclcollision.txt fclspace.txt

Now I will try again if I can do it with --base_link=2 --eelink=8 --freeindex=8. And if I have time some other combinations too.

EDIT: This hasn't worked, but it's too late, I will continue tomorrow

mikaelarguedas commented 8 years ago

My bad, it seems that you need to include and not (http://stackoverflow.com/questions/28914711/getting-error-shared-ptr-in-namespace-std-does-not-name-a-type)

What is surprising is that you get a result for gripper = (0,0,0) (aka wrist) but you have an error when you use the wrist at the end of the chain... It should be the exact same IK solution in theory. And it's also weird that it works for one arm but not for the other one.

lluissalord commented 8 years ago

For the rigth arm I have get the IK file with the gripper = (0,0,0) and --baselink=19 --eelink=26 --freeindex=22 (RShoulder, r_gripper i RElbow) instead of Wrist_Roll I have used Elbow. It's supposed that Elbow and Wrist_Roll are doing the same rotation so it should be the same. But I think is better to have the freeindex closer to the end-effector, but I'm not sure.

I don't know yet because I haven't create the plugin and test with the IK files, but I think that the best solution should be --baselink=0 --eelink=8 --freeindex=6 (for the left arm) and --baselink=0 --eelink=25 --freeindex=23 (for the right arm). Then we will have the pose in the coord of the base_link frame of Romeo. But for me is not working I don't know why.

lluissalord commented 8 years ago

Now I'm going to change the names and all stuff to can make the plugins but for what I read here #13 the changes that I'll make, will they be replace when you re-generate the model?

mikaelarguedas commented 8 years ago

One of the reasons it may be not working is that if you start from the base_link you'll have more that 6 joints in your kinematic chain. It shouldn't be an issue to have all the positions computed with respect to another frame. tf will do the transformation job for you when you need it. Having the last joint of the kinematic chain free will facilitate your compatibility with @nlyubova 's work with moveit_simple_grasps where the last wrist joint is considered as free too.

Regarding the file renaming, in the package romeo_description you should have very few places to change the name. I can't tell you yet if this is going to be done for you in the urdf/xacro file generation in the future. It shouldn't be an issue to change the robot name at generation time for romeo, but for the other robots (I'm thinking Nao particularly) there are several versions of the robot and the version number is integrated in the name and it may be meaningful to keep this information. But maybe it's just redundant information given that files are generated in folders that already contain the version number in the name. I don't have a strong opinion about it. On the moveit_config packages files it may be less straight forward to do it. One of the reasons is that (for nao for example) we have several versions of the robot and for now a single moveit_config package. I don't know how it will be extended in the future to support several versions so for now you'll need to change this by hand. In this case too there are not many places where it needs to be changed. Please note that the files romeoH37.srdf, romeoH37_moveit_controller_manager.launch.xml and romeoH37_moveit_sensor_manager.launch.xml should be renamed as well for full compatibility.

Good luck !

lluissalord commented 8 years ago

Now I'm with the mobile so I can answer all the things will all the details but I'll try. For what I have seen start at base_link, torso or Shoulder is like the same because they are fixed joints so it's only a translation. The think is that when I used the one from LShoulder to l_gripper it seems that OMPL look for the first link and joint in the model of IKfast to put as a reference. And it doesn't recognize LShoulderPitch as a joint. So I think that maybe using torso as a --baselink will fix that. Besides, in the file RomeuH37.srdf for the group took the torso as a base_link.

According to that and to put the last joint as free the best config should be --baselink=1 --eelink=8 and --freeindex=8 for the left_arm group and the respective 1,25,25 (I say that from memory not sure) for the right_arm group. These configs haven't worked for me if @mikaelarguedas could try and if it works send the Ik files would be greate, then if you want I can make the plugin packages if you want.

Mention that at least to test with MoveIt and not with the real robot. Only changing the name of the robot in the files RomeoH37.urdf and RomeuH37.srdf it was enough. But as I said before after that it gave me the error with LShoulderPitch.

mikaelarguedas commented 8 years ago

Yeah for the renaming I just wanted to keep consistency across files/packages otherwise we'll get confused at some point. But changing only in a subset of files should be sufficient for testing.

I think I do have a left arm ikfast file generated with that config. And the ikfast plugin that goes with it. From memory I tested it in moveit and could generate trajectories in ~3sec for most random_valid positions. I didn't test it for approximate positions so it's certainly a bad indication of the performance of the planner. You may need to move the joint with children l_gripper out of the left_arm group in the moveit configuration though.

I don't have my laptop right now so I'll take a look tonight and post the files if it's indeed the same config.

lluissalord commented 8 years ago

Now I've seen that if we want to have the joint L/RShoulderPitch we have to use as base_link the torso or base_link. Use the command openrave0.9-robot.py romeo.dae --info joints it gives the information of the joints like this:

name                     joint_index dof_index parent_link       child_link        mimic
----------------------------------------------------------------------------------------
LShoulderPitch           0           0         torso             LShoulder              
LShoulderYaw             1           1         LShoulder         LShoulderYaw_link      
LElbowRoll               2           2         LShoulderYaw_link LForeArm               
LElbowYaw                3           3         LForeArm          LElbow                 
LWristRoll               4           4         LElbow            LWristRoll_link        
LWristYaw                5           5         LWristRoll_link   LWristYaw_link         
LWristPitch              6           6         LWristYaw_link    l_wrist                
LHand                    7           7         l_wrist           l_gripper

So if we start with the --baselink=2 (LShoulder) we don't include the joint LShoulderPitch.