Use fullbody controller for the nextage real robot (#626)

* Use different interface when using gazebo

Author: Michi-Tsubaki

skrobot/interfaces/ros/nextage.py

@@ -1,25 +1,43 @@
 import actionlib
 import control_msgs.msg
+import rospy
+import trajectory_msgs.msg
 
 from .base import ROSRobotInterfaceBase
 
 
 class NextageROSRobotInterface(ROSRobotInterfaceBase):
-
     def __init__(self, *args, **kwargs):
+        self.on_gazebo = rospy.get_param('/gazebo/time_step', None) is not None \
+                         and rospy.get_param('/torso_controller/type', None) is not None
+
+        if self.on_gazebo:
+            rospy.loginfo("Gazebo environment detected")
+
+        self.lhand = None
+        self.rhand = None
+
         super(NextageROSRobotInterface, self).__init__(*args, **kwargs)
 
-        self.rarm_move = actionlib.SimpleActionClient(
-            '/rarm_controller/follow_joint_trajectory_action',
-            control_msgs.msg.FollowJointTrajectoryAction
-        )
-        self.rarm_move.wait_for_server()
+    def _init_lhand(self):
+        if self.lhand is None:
+            self.lhand = LHandInterface()
+        return self.lhand
 
-        self.larm_move = actionlib.SimpleActionClient(
-            '/larm_controller/follow_joint_trajectory_action',
-            control_msgs.msg.FollowJointTrajectoryAction
+    def _init_rhand(self):
+        if self.rhand is None:
+            self.rhand = RHandInterface()
+        return self.rhand
+
+    @property
+    def fullbody_controller(self):
+        return dict(
+            controller_type='fullbody_controller',
+            controller_action='/fullbody_controller/follow_joint_trajectory_action',
+            controller_state='/fullbody_controller/state',
+            action_type=control_msgs.msg.FollowJointTrajectoryAction,
+            joint_names=[j.name for j in self.robot.joint_list],
         )
-        self.larm_move.wait_for_server()
 
     @property
     def rarm_controller(self):
@@ -41,19 +59,137 @@ def larm_controller(self):
             joint_names=[j.name for j in self.robot.larm.joint_list],
         )
 
+    @property
+    def torso_controller(self):
+        return dict(
+            controller_type='torso_controller',
+            controller_action='/torso_controller/follow_joint_trajectory_action',
+            controller_state='/torso_controller/state',
+            action_type=control_msgs.msg.FollowJointTrajectoryAction,
+            joint_names=[j.name for j in self.robot.torso.joint_list],
+        )
+
+    @property
+    def head_controller(self):
+        return dict(
+            controller_type='head_controller',
+            controller_action='/head_controller/follow_joint_trajectory_action',
+            controller_state='/head_controller/state',
+            action_type=control_msgs.msg.FollowJointTrajectoryAction,
+            joint_names=[j.name for j in self.robot.head.joint_list],
+        )
+
     def default_controller(self):
-        return [self.rarm_controller, self.larm_controller]
+        if self.on_gazebo:
+            return [self.larm_controller, self.rarm_controller,
+                    self.torso_controller, self.head_controller]
+        else:
+            return [self.fullbody_controller]
+
+    def start_grasp(self, arm='arms', **kwargs):
+        if arm == 'larm':
+            self._init_lhand().start_grasp(**kwargs)
+        elif arm == 'rarm':
+            self._init_rhand().start_grasp(**kwargs)
+        elif arm == 'arms':
+            self._init_lhand().start_grasp(**kwargs)
+            self._init_rhand().start_grasp(**kwargs)
+
+    def stop_grasp(self, arm='arms', **kwargs):
+        if arm == 'larm':
+            self._init_lhand().stop_grasp(**kwargs)
+        elif arm == 'rarm':
+            self._init_rhand().stop_grasp(**kwargs)
+        elif arm == 'arms':
+            self._init_lhand().stop_grasp(**kwargs)
+            self._init_rhand().stop_grasp(**kwargs)
 
-    def move_arm(self, trajectory, arm='rarm', wait=True):
+    def open_forceps(self, arm='arms', **kwargs):
+        if arm == 'larm':
+            self._init_lhand().open_forceps(**kwargs)
+        elif arm == 'arms':
+            self._init_lhand().open_forceps(**kwargs)
+
+    def close_forceps(self, arm='arms', **kwargs):
+        if arm == 'larm':
+            self._init_lhand().close_forceps(**kwargs)
+        elif arm == 'arms':
+            self._init_lhand().close_forceps(**kwargs)
+
+    def open_holder(self, arm='arms', **kwargs):
+        if arm == 'rarm':
+            self._init_rhand().open_holder(**kwargs)
+        elif arm == 'arms':
+            self._init_rhand().open_holder(**kwargs)
+
+    def close_holder(self, arm='arms', **kwargs):
         if arm == 'rarm':
-            self.send_trajectory(self.rarm_move, trajectory, wait)
-        elif arm == 'larm':
-            self.send_trajectory(self.larm_move, trajectory, wait)
+            self._init_rhand().close_holder(**kwargs)
+        elif arm == 'arms':
+            self._init_rhand().close_holder(**kwargs)
+
+
+class LHandInterface:
+    def __init__(self):
+        self.action_client = actionlib.SimpleActionClient(
+            "/lhand/position_joint_trajectory_controller/follow_joint_trajectory",
+            control_msgs.msg.FollowJointTrajectoryAction
+        )
+        if not self.action_client.wait_for_server(rospy.Duration(5)):
+            rospy.logwarn("LHand action server not available")
 
-    def send_trajectory(self, client, trajectory, wait=True):
+    def move_hand(self, grasp_angle, wait=True, tm=1.0):
         goal = control_msgs.msg.FollowJointTrajectoryGoal()
-        goal.trajectory = trajectory
+        goal.trajectory.joint_names = ["lhand_joint"]
+        point = trajectory_msgs.msg.JointTrajectoryPoint()
+        point.positions = [grasp_angle]
+        point.time_from_start = rospy.Duration(tm)
+        goal.trajectory.points = [point]
+        self.action_client.send_goal(goal)
         if wait:
-            client.send_goal_and_wait(goal)
-        else:
-            client.send_goal(goal)
+            self.action_client.wait_for_result(rospy.Duration(tm + 5.0))
+
+    def start_grasp(self, **kwargs):
+        return self.move_hand(-2.7, **kwargs)
+
+    def stop_grasp(self, **kwargs):
+        return self.move_hand(0.0, **kwargs)
+
+    def open_forceps(self, wait=False, tm=0.2):
+        return self.move_hand(-2.2, wait, tm)
+
+    def close_forceps(self, wait=False, tm=0.2):
+        return self.move_hand(-3.2, wait, tm)
+
+
+class RHandInterface:
+    def __init__(self):
+        self.action_client = actionlib.SimpleActionClient(
+            "/rhand/position_joint_trajectory_controller/follow_joint_trajectory",
+            control_msgs.msg.FollowJointTrajectoryAction
+        )
+        if not self.action_client.wait_for_server(rospy.Duration(5)):
+            rospy.logwarn("RHand action server not available")
+
+    def move_hand(self, grasp_angle, wait=True, tm=1.0):
+        goal = control_msgs.msg.FollowJointTrajectoryGoal()
+        goal.trajectory.joint_names = ["rhand_joint"]
+        point = trajectory_msgs.msg.JointTrajectoryPoint()
+        point.positions = [grasp_angle]
+        point.time_from_start = rospy.Duration(tm)
+        goal.trajectory.points = [point]
+        self.action_client.send_goal(goal)
+        if wait:
+            self.action_client.wait_for_result(rospy.Duration(tm + 5.0))
+
+    def start_grasp(self, **kwargs):
+        return self.move_hand(-2.7, **kwargs)
+
+    def stop_grasp(self, **kwargs):
+        return self.move_hand(0.0, **kwargs)
+
+    def open_holder(self, wait=True, tm=0.2):
+        return self.move_hand(-0.20, wait, tm)
+
+    def close_holder(self, wait=True, tm=0.2):
+        return self.move_hand(0.08, wait, tm)

skrobot/models/nextage.py

@@ -97,3 +97,11 @@ def head(self):
         model = RobotModel(link_list=links, joint_list=joints)
         model.end_coords = self.head_end_coords
         return model
+
+    @cached_property
+    def torso(self):
+        link_names = ["CHEST_JOINT0_Link"]
+        links = [getattr(self, n) for n in link_names]
+        joints = [l.joint for l in links]
+        model = RobotModel(link_list=links, joint_list=joints)
+        return model