PDA

View Full Version : Simple_Arms



Peter_heim
06-12-2011, 06:48 AM
Hi Fergs
I just checked out the simple_arms package and it complies on my system
but when i try simple_arm_server_test.py i keep getting false
my arm is a 5 dof with the servos (AX 12) just connected together with bioloid frames so it only has a 40cm reach when i use
simple_arm_server_test.py 0.2 0.2 0.0 0.0 0.0it returns false as will various other numbers. with the above numbers i assume the end should be 20cm from from the center of my deck_link(root) and 20cm to the left(?) and on the hight as the deck. But the last 2 i take it as the position you want the end to be in? what would be the values be radians or some other?

Regards Peter

lnxfergy
06-12-2011, 12:46 PM
Hi Fergs
I just checked out the simple_arms package and it complies on my system
but when i try simple_arm_server_test.py i keep getting false
my arm is a 5 dof with the servos (AX 12) just connected together with bioloid frames so it only has a 40cm reach when i use
simple_arm_server_test.py 0.2 0.2 0.0 0.0 0.0it returns false as will various other numbers. with the above numbers i assume the end should be 20cm from from the center of my deck_link(root) and 20cm to the left(?) and on the hight as the deck. But the last 2 i take it as the position you want the end to be in? what would be the values be radians or some other?

Regards Peter

The usage definition is:

"simple_arm_server_test.py x y z wrist_pitch [wrist_roll=0.0 frame_id='base_link']"

The parameters in [] are optional, and have defaults, but you might need to override them. So, you need to enter an x, y, z as you are, but wrist_pitch is probably throwing you off.

An easy way to see what the wrist pitch is: "rosrun tf tf_echo base_link gripper_link" will give you all the X, Y, Z and pitch that you need as a starting point.

I'll be working on a tutorial later today on using this (you'll note, it was only check into SVN about 24 hours ago). I still need to check in a few updates (actually use the time requested, search a bit within the wrist_pitch range).

-Fergs

lnxfergy
06-12-2011, 01:17 PM
Docs on the server are now up: http://www.ros.org/wiki/simple_arm_server

Pi Robot
06-16-2011, 08:27 AM
Hey Fergs and Peter,

I am giving Fergs' new simple_arm_server a test run on one of Pi Robot's 6-dof arms. First of all--just to check--can simple_arm_server be used on a 6-dof arm? (I see the emphasis is on 4 and 5 dof). Assuming it can, I'm running into a problem of the inverse kinematics not finding any solutions even when I give it the current pose as a target. I'll include all the relevant files below but this is what I am trying:



Launch the arbotix_python and traj_controller nodes along with my pi_robot.urdf.xacro description.
Test servo movement with controllerGUI.py (tests OK)
Run simple_arm_server with parameters arm_dof="6", root="left_shoulder_link", tip="left_hand_link"
rosrun tf tf_echo base_link left_hand_link which gives me the output:

- Translation: [0.143, 0.101, 0.058]
- Rotation: in Quaternion [-0.012, -0.152, -0.093, 0.984]
in RPY [0.005, -0.307, -0.188]
Run the simple_arm_server_test.py script with the above values like this:

rosrun simple_arm_server simple_arm_server_test.py 0.143 0.101 0.058 -0.307

which gives me output:
Success: False

I've tried each of the RPY values for pitch but I always get back Success=False.

Here now are my various files:

arbotix.launch:


<launch>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find pi_description)/urdf/pi_robot.urdf.xacro'" />

<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
<param name="publish_frequency" value="20"/>
</node>

<node name="arbotix" pkg="arbotix_python" type="driver.py" output="screen">
<rosparam file="$(find pi_arbotix)/params/arbotix_params.yaml" command="load" />
</node>

<node name="arm_controller" pkg="arbotix_controllers" type="traj_controller.py">
<rosparam param="joints">[left_shoulder_lift_joint, left_shoulder_pan_joint, left_arm_roll_joint, left_elbow_joint, left_forearm_joint, left_wrist_joint]</rosparam>
</node>

<node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostic_aggregator" >
<rosparam command="load" file="$(find pi_arbotix)/params/diagnostics.yaml" />
</node>

<node pkg="robot_monitor" type="robot_monitor" name="robot_monitor" />
</launch>arbotix_params.yaml


baud: 115200
rate: 50
read_rate: 10
write_rate: 20
sync_read: True
sync_write: True

dynamixels: {
head_pan_joint: {id: 1, max_speed: 360, min_angle: -145, max_angle: 145},
head_tilt_joint: {id: 2, max_speed: 360, min_angle: -90, max_angle: 90},
right_shoulder_lift_joint: {id: 17, neutral: 512, max_speed: 50, invert: False},
right_shoulder_pan_joint: {id: 18, neutral: 512, max_speed: 50, invert: False},
right_arm_roll_joint: {id: 5, max_speed: 200},
right_elbow_joint: {id: 13, max_speed: 200, invert: True},
right_forearm_joint: {id: 3, max_speed: 200, invert: True},
right_wrist_joint: {id: 6, max_speed: 200},
left_shoulder_lift_joint: {id: 15, neutral: 512, max_speed: 50, invert: False},
left_shoulder_pan_joint: {id: 16, neutral: 512, max_speed: 50, invert: False},
left_arm_roll_joint: {id: 9, max_speed: 360},
left_elbow_joint: {id: 12, max_speed: 360},
left_forearm_joint: {id: 4, max_speed: 360},
left_wrist_joint: {id: 10, max_speed: 360, invert: False},
torso_joint: {id: 11, max_speed: 360}
}simple_arm_server.launch


<launch>
<include file="$(find simple_arm_server)/launch/arm_server.launch">
<arg name="arm_dof" value="6" />
<arg name="root_name" value="left_shoulder_link" />
<arg name="tip_name" value="left_hand_link" />
</include>
</launch>

--patrick

lnxfergy
06-16-2011, 12:56 PM
Patrick, it should work fine with 6DOF. Are you sure that your joint limits in URDF (which the arm_kinematics package will use) correspond to those in your arbotix.yaml (which the controllerGUI and trajectory controller are using). If not, it might be the case that your URDF limits are tighter than the yaml ones?

-Fergs

Pi Robot
06-16-2011, 05:37 PM
Hi Fergs,

I just checked the limits in my URDF description and they are set to -3.14 to 3.14 for all arm joints. I also did a test using just the arm_kinematics package outside of simple_arm_server but using the joint_state_publisher instead of the real joint values since I'm currently way from the robot. At least in this case, I get a valid IK solution when feeding an FK solution as the target. But let me try it on the real robot when I get home tonight and I'll see if that still works.

--patrick

lnxfergy
06-16-2011, 06:09 PM
Can we also get the screen output from the arm server launch file? it should output a pile of stuff.

-Fergs

lnxfergy
06-16-2011, 09:30 PM
Also note, I just committed a huge change, that makes simple arm server much better! We now support multiple points in the message. It will probably take a few hours before all the info is updated on the wiki (due to the indexer).

I'm working on the docs/tutorials right now.

-Fergs

Peter_heim
06-17-2011, 06:56 AM
Hi Fergs
Thanks for the help it now works with the following

simple_arm_server simple_arm_server_test.py 0.396 0.065 0.472 1.0

Peter

Pi Robot
06-17-2011, 11:50 AM
Can we also get the screen output from the arm server launch file? it should output a pile of stuff.
-Fergs

OK, finally back in front of the robot. Here are the screen messages when I run the following commands:


$ rosrun tf tf_echo base_link left_hand_link
At time 1308329104.704
- Translation: [0.071, 0.128, 0.050]
- Rotation: in Quaternion [-0.093, -0.025, 0.073, 0.993]
in RPY [-0.190, -0.036, 0.150]
$ rosrun simple_arm_server simple_arm_server_test.py 0.071 0.128 0.050 -0.036
success: FalseAnd in the simple_arm_server window:


core service [/rosout] found
process[arm_kinematics-1]: started with pid [18018]
process[simple_arm_server-2]: started with pid [18019]
[INFO] [WallTime: 1308328712.829041] simple_arm_server node started
[INFO] [WallTime: 1308328904.791556] Move arm to position:
x: 0.071
y: 0.128
z: 0.05
orientation:
x: 0.0
y: -0.0179990280157
z: 0.0
w: 0.999838004374
[INFO] [WallTime: 1308328904.835017] solution:
joint_state:
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
name: []
position: []
velocity: []
effort: []
multi_dof_joint_state:
stamp:
secs: 0
nsecs: 0
joint_names: []
frame_ids: []
child_frame_ids: []
poses: []
error_code:
val: -31The only thing I can think of is that my arbotix_params.yaml file has a couple of joints with the invert parameter set to True. Could this part of the problem?

Also, my URDF/Xacro file can be found here:

http://code.google.com/p/pi-robot-ros-pkg/source/browse/trunk/experimental/ros_by_example/pi_description/urdf/pi_robot.urdf.xacro

--patrick

lnxfergy
06-17-2011, 12:33 PM
Whoops! The server can certainly handle 6DOF, but the test program didn't (we have to pass a yaw value, 0.0 isn't gonna work).

Check out the latest trunk, and try this:



rosrun simple_arm_server simple_arm_server_test.py 0.71 0.128 0.05 -0.036 -0.190 0.150


-Fergs

Pi Robot
06-17-2011, 01:11 PM
Thanks Fergs--alas, I am still not getting a solution. I now have revision 383 and here is what I just tried:


$ rosrun tf tf_echo base_link left_hand_link
- Translation: [0.109, 0.123, 0.050]
- Rotation: in Quaternion [-0.088, -0.009, -0.220, 0.971]
in RPY [-0.167, -0.057, -0.441]

$ rosrun simple_arm_server simple_arm_server_test.py 0.109 0.123 0.050 -0.057 -0.167 -0.441
success: FalseDo I have the order of the angle parameters right?

--patrick

Pi Robot
06-17-2011, 06:21 PM
Well, it looks like I have the same problem even when just using the arm_kinematics solver on its own. So I don't think anything needs to be fixed in your simple_arm_server. Not sure why the solver can't get the solution when given the same pose as the forward kinematics produces from the current joint positions, but I'll have to dig deeper into my setup...

--patrick

Pi Robot
06-17-2011, 11:19 PM
Good news--I am now getting IK solutions using both arm_kinematics on its own and simple_arm_server. I think I tracked the problem down to asking for solutions too far away from the starting point. It appears that the default seed position for the joint angles is to set them all to 0. When I set the target position too far from this seed position, both arm_kinematics and simple_arm_server fail. If I then choose seed values closer to the solution, both packages succeed.

--patrick

Pi Robot
06-20-2011, 05:45 PM
Hey Fergs,

I thought I was home free with the arm kinematics stuff on Pi Robot. However, I am now doing a more thorough test by passively moving the arm joints through their range of motion and asking the IK solver to give me a solution for the current position of the hand. I use the current joint states as the seed values as I figured that would make it easier. I'm discovering many non-extreme configurations where the IK solver is unable to find a solution. The problem configurations seem clearly related to specific angle ranges at specific joints. For example, the solver always fails to find a solution if I move the elbow joint past the neutral point in one direction, but move the elbow in the other direction past neutral and I always get a valid solution no matter how far I bend it! Similarly, if I lift the whole arm using the shoulder lift servo, I reach a point about 45 degrees from vertical where the solver then consistently fails from that point and higher.

I changed the angle limits from -3.14/3.14 to -6.28/6.28 on all arm servos but that does not seem to have an effect. Any ideas what might be going on?

Thanks!
patrick

Pi Robot
06-21-2011, 12:38 PM
I just discovered an important update to my issue so don't waste any time on it at this point. I discovered that if I take the real servos and ArbotiX out of the equation and just use joint_state_publisher along with the slider GUI to position the joints in RViz, I get IK solutions for all configurations of the arm, no matter how contorted I make it. So there is something about the joint states being published by arbotix_python or the readings from my servos that is causing the trouble. Stay tuned...

--patrick

lnxfergy
06-21-2011, 12:49 PM
I just discovered an important update to my issue so don't waste any time on it at this point. I discovered that if I take the real servos and ArbotiX out of the equation and just use joint_state_publisher along with the slider GUI to position the joints in RViz, I get IK solutions for all configurations of the arm, no matter how contorted I make it. So there is something about the joint states being published by arbotix_python or the readings from my servos that is causing the trouble. Stay tuned...

--patrick

Make sure all your servos are reading correctly. If a read fails, it returns -1 (where normally you have 0-1023). This currently gets blindly converted into a radian angle (something like +/-2.7.....), which is certainly out of range.

-Fergs

Pi Robot
06-21-2011, 01:13 PM
Thanks Fergs--that sounds like a definite possibility since I don't think I have ever had completely noise-free servo performance. I'll check it tonight when I'm back in front of the robot.

--patrick

Pi Robot
06-23-2011, 08:56 AM
At last! It wasn't noise in the servos, it was noise in my brain. Turns out I had defined the tip link in arm_kinematics to be "left_finger_link" instead of "left_hand_link". Once I made the correction, I started getting IK solutions for all arm configurations with the ArbotiX publishing the joint states.

--patrick

zhayantian
04-25-2014, 10:06 AM
Hi fergs,
I have phantomx pincher arm, and turtlebot2, with Groovy.
I got strange problems in rosrunning simple_arm_server simple_arm_server_test.py [...(params collected from tf_echo)]. There was no text shown after rosrunning it, and arm didn't move.
But the gripper should move to point(x,y,z) as what was given. Pls help me and tell me if more info are needed, thanks!

lnxfergy
04-26-2014, 12:41 AM
As stated on the README in the source (https://github.com/vanadiumlabs/simple_arms), simple arms was for Fuerte and earlier (arm_navigation). I don't think I ever really tested on Groovy, because by then it was obsolete as MoveIt integrates an entire semantic pick-and-place system and a IkFast solution would do exactly what the simple arm server did for IK.

There are a huge number of possible things that could be wrong here -- arm_navigation was barely supported under groovy even. I unfortunately have only so many hours to dedicate to open source, and rarely do these hours go towards things that are already deprecated and end-of-life. Good luck if you continue to go down this path, but I would seriously take a look at the newer options like MoveIt/IkFast, as even Groovy itself is about to be EOL in another month or so.

-Fergs