ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

nao_teleop BodyPoseActions fail

asked 2012-05-15 03:45:15 -0500

hawesie gravatar image

updated 2012-05-15 21:41:05 -0500

I'm using the nao_teleop system. I can enable the controller, move the head around (the instructions for this on the wiki are incorrect -- btn 5 needs to be help while analogue sticks are used) and control walking if the robot is already standing, but I the system fails to execute any of the body pose actions (init or crouch). The output I see is as follows. Can anyone point towards a solution? Let me know if more output is necessary too.

EDIT: Apologies for the lack of information (I was running out of the office and failed to engage my brain)! My setup:

  • naoqi-sdk-1.12.3-linux32 on the H21 Nao (sprung hands)
  • Ubuntu Oneiric (in a 32 bit chroot in a 64 bit install)
  • ROS Fuerte, installed from binary packages
  • nao_teleop was fetched from svn using rosinstall (as instructed by the wiki), at revision 2727 (last changed 2340)
NODES
  /
    joy_node (joy/joy_node)
    nao_controller (nao_driver/nao_controller.py)
    nao_sensors (nao_driver/nao_sensors.py)
    nao_walker (nao_driver/nao_walker.py)
    pose_manager (nao_remote/pose_manager.py)
    remap_odometry (nao_remote/remap_odometry)
    robot_state_publisher (robot_state_publisher/state_publisher)
    teleop_nao_joy (nao_teleop/teleop_nao_joy)

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[nao_walker-1]: started with pid [18467]
process[nao_sensors-2]: started with pid [18468]
process[nao_controller-3]: started with pid [18469]
process[robot_state_publisher-4]: started with pid [18470]
process[remap_odometry-5]: started with pid [18490]
process[pose_manager-6]: started with pid [18525]
process[teleop_nao_joy-7]: started with pid [18528]
process[joy_node-8]: started with pid [18568]
[INFO] [WallTime: 1337087278.686138] Connecting to NaoQi at 192.168.0.6:9559
[INFO ] Starting ALNetwork
[INFO] [WallTime: 1337087278.774109] Connecting to NaoQi at 192.168.0.6:9559
[INFO ] Starting ALNetwork
[INFO] [WallTime: 1337087278.961661] Connecting to NaoQi at 192.168.0.6:9559
[INFO ] Starting ALNetwork
[INFO ] NAOqi is listening on 127.0.0.1:54010
[INFO] [WallTime: 1337087279.133906] for_iros2011 = 0
say Walker online ...
[INFO ] NAOqi is listening on 127.0.0.1:54011
...done
[INFO] [WallTime: 1337087279.181416] nao_walker initialized
[INFO] [WallTime: 1337087279.181596] nao_walker running...
[INFO ] NAOqi is listening on 127.0.0.1:54012
[INFO] [WallTime: 1337087279.573459] nao_controller initialized
[INFO] [WallTime: 1337087279.573854] nao_controller running...
[INFO] [WallTime: 1337087282.363589] nao_sensors initialized
[INFO] [WallTime: 1337087283.775101] Body stiffness enabled
say Gamepad control enabled ...
...done
[INFO] [WallTime: 1337087342.310645] JointTrajectory action executing
[ERROR] [WallTime: 1337087342.333748] Exception in your execute callback:   ALMotion::angleInterpolation
        ALBroker::methodCall: method: angleInterpolation, params: [["Body"], [[0], [0], [1.545], [0.33], [-1.57], [-0.486], [0], [0], [-0.3], [0.057], [-0.744], [2.192], [-1.122], [-0.035], [-0.3], [0.057], [-0.744], [2.192], [-1.122], [-0.035], [1.545], [-0.33], [1.57], [0.486], [0], [0]], 1.5, true]
        ALMotion::angleInterpolation
        ALMotion::angleInterpolation
    Expected the angles series to be of the same size as the joint names
Traceback (most recent call last):
  File "/opt/ros/fuerte/lib/python2.7/dist-packages/actionlib/simple_action_server.py", line 292, in executeLoop
    self.execute_callback(goal)
  File "/opt/ros/local/stacks/nao_robot/nao_driver/scripts/nao_controller.py", line 183, in executeJointTrajectoryAction
    self.motionProxy.angleInterpolation(names, angles, times, (goal.relative ...
(more)
edit retag flag offensive close merge delete

Comments

1

Which NaoQI version? Running on which system? Which ROS version? Are you using the latest version of nao_driver from SVN?

AHornung gravatar image AHornung  ( 2012-05-15 06:01:33 -0500 )edit

Heh, I really did underspecify things, sorry!

hawesie gravatar image hawesie  ( 2012-05-15 08:43:29 -0500 )edit

Added even more info: H21 model, perhaps the lack of hands is related?

hawesie gravatar image hawesie  ( 2012-05-15 23:29:00 -0500 )edit

Yes, that could indeed be the case! At the beginning of "executeJointTrajectoryAction", can you try to strip the four values corresponding to hands and wrists? You can have a look at nao_sensors.py lines 104-108 which does the reverse (fill the values if the array is too short).

AHornung gravatar image AHornung  ( 2012-05-16 06:25:45 -0500 )edit

You could also try to replace the single "Body" joint name (which will expand to all of you body's joints) with a complete list of all H25 joints in nao_remote/config/base_poses.yaml. Then the arrays have the same length and maybe NaoQI will just ignore the extra values.

AHornung gravatar image AHornung  ( 2012-05-16 06:29:09 -0500 )edit

Thanks, I'll check these today. Also, I raised an issue on your Google code site. Let me know if you'd prefer it moved to here. I'm never quite sure where the best place to put these things is.

hawesie gravatar image hawesie  ( 2012-05-16 21:37:37 -0500 )edit

Either is fine. A tracker item at google code will make sure that I won't forget it ;)

AHornung gravatar image AHornung  ( 2012-05-17 05:16:46 -0500 )edit
add a comment

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-05-17 00:40:16 -0500

hawesie gravatar image

Yep, it was the difference in joint count between the H25 and H21 models. There are at least 2 fixes to make this work. The first, which allows the H21 to work with no alterations to configuration files is to patch nao_drivers/scripts/nao_controller.py with the following (svn) diff:

Index: scripts/nao_controller.py
===================================================================
--- scripts/nao_controller.py   (revision 2727)
+++ scripts/nao_controller.py   (working copy)
@@ -65,6 +65,9 @@

         self.connectNaoQi()

+        # get number of available joints, used to distinguish between H21 and H25 later
+        self.availableJoints = len(self.motionProxy.getJointNames("Body"))
+
         # store the number of joints in each motion chain and collection, used for sanity checks
         self.collectionSize = {}
         for collectionName in ['Head', 'LArm', 'LLeg', 'RLeg', 'RArm', 'Body', 'BodyJoints', 'BodyActuators']:
@@ -176,6 +179,14 @@

         names, angles, times = self.jointTrajectoryGoalMsgToAL(goal)

+        #strip 6,7 and last 2 from angles if the pose was for H25 but we're running an H21
+        if len(angles) > self.availableJoints:
+            rospy.loginfo("stripping %d down to %d", len(angles), self.availableJoints)
+            angles.pop(6)
+            angles.pop(7)
+            angles.pop()
+            angles.pop()
+
         rospy.logdebug("Received trajectory for joints: %s times: %s", str(names), str(times))     
         rospy.logdebug("Trajectory angles: %s", str(angles))

The second approach is for the end user to provide their own yaml file containing poses to the pose_manager. This is preferred for me because the default crouch action provided in pose_manager.py causes my robot to topple over backwards. I don't know if this is true for all H21s or all H*s or just my robot. I put a file in my nao_demo package called config/bham_poses.yaml. I loaded parameters from this in my launch file last (to override the params set in the nao_remote.launch). My launch file therefore contains:

<rosparam file="$(find nao_demo)/config/bham_poses.yaml" command="load" ns="/pose_manager/poses"/>

And the contents of that file is as follows. These poses are hopefully good for all H21 robots.

### some basic poses to load for pose_manager.py for H21 ###

init: # "ready for anything"
  joint_names: ["Body"]
  time_from_start: 1.5
  positions: [0.0,0.0, 1.39, 0.34, -1.39, -1.04, 0.0, 0.0, 0.0, 0.0, -0.43, 0.69, -0.34, 0.0, 0.0, 0.0, -0.43, 0.69, -0.34, 0.0, 1.39, -0.34, 1.39, 1.04, 0.0, 0.0]
  positions: [0.0, 0.0, 1.39, 0.34, -1.39, -1.04, 0.0, 0.0, -0.43, 0.69, -0.34, 0.0, 0.0, 0.0, -0.43, 0.69, -0.34, 0.0, 1.39, -0.34, 1.39, 1.04]

zero:  
  joint_names: ["Body"]
  time_from_start: 1.5
  positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 

crouch:  
  joint_names: ["Body"]
  time_from_start: 1.5
  positions: [0.00762803852558136, 0.04444403946399689, 1.4158400297164917 ...
(more)
edit flag offensive delete link more

Comments

Thanks for the patch! I'll integrate it asap. Indeed, overriding the parameters with your own configs is the preferred way for users. Particularly for slightly varying hardware which may cause different behavior (the crouch works good for both v3 and v4 H25 Naos in our lab).

AHornung gravatar image AHornung  ( 2012-05-17 05:20:10 -0500 )edit

Patch is now integrated into nao_controller (SVN trunk). There's now an additional tool to test single poses in nao_remote as well (e.g. call "execute_pose.py init").

AHornung gravatar image AHornung  ( 2012-05-20 10:23:19 -0500 )edit
add a comment

Question Tools

subscribe to rss feed

Stats

Asked: 2012-05-15 03:45:15 -0500

Seen: 1,079 times

Last updated: May 17 '12

Related questions

nao_teleop not working [closed]

ERROR: the following packages/stacks could not have their rosdep keys resolved to system dependencies:

How to run gazebo with no gui? [closed]

How to remove/uninstall fuerte completely?

Error building Fuerte on OSX

big delay between publisher and subscriber !

Gazebo 1.0.x and CUDA in Furete

Running ros-based code on Nao Robot

What's the package-node to retrieve FSR Nao sensors status?

Running Gazebo headless in Fuerte over SSH

ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
about | faq | help | privacy policy | terms of service
Powered by Askbot version 0.10.2