Ask Your Question

Revision history [back]

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, 0.31442803144454956, -1.3852440118789673, -1.0200680494308472, -0.010696038603782654, 0.06446996331214905, -0.8789400458335876, 2.112546443939209, -1.1305999755859375, -0.07972604036331177, -0.010696038603782654, -0.05211403965950012, -0.8406739830970764, 2.112546443939209, -1.186300277709961, 0.0368579626083374, 1.5340420007705688, -0.3053079843521118, 1.3912960290908813, 1.0247540473937988]