Add support for multiple kinemtaics chains.

.gitmodules

@@ -1,3 +1,3 @@
 [submodule "relaxed_ik_core"]
 	path = relaxed_ik_core
-	url = git@github.com:uwgraphics/relaxed_ik_core.git
+	url = https://github.com/hshi74/relaxed_ik_core.git

scripts/keyboard_input.py

@@ -1,94 +1,109 @@
 #!/usr/bin/python3
 
+import numpy as np
 import readchar
-import rospy
 import rospkg
-from geometry_msgs.msg import PoseStamped, Vector3Stamped, QuaternionStamped, Pose, Twist
-from std_msgs.msg import Bool
-from relaxed_ik_ros1.msg import EEPoseGoals, EEVelGoals
+import rospy
 import transformations as T
-from robot import Robot
+from geometry_msgs.msg import (
+    Pose,
+    PoseStamped,
+    QuaternionStamped,
+    Twist,
+    Vector3Stamped,
+)
 from pynput import keyboard
+from robot import Robot
+from std_msgs.msg import Bool
+
+from relaxed_ik_ros1.msg import EEPoseGoals, EEVelGoals
+
+path_to_src = rospkg.RosPack().get_path("relaxed_ik_ros1") + "/relaxed_ik_core"
 
-path_to_src = rospkg.RosPack().get_path('relaxed_ik_ros1') + '/relaxed_ik_core'
 
 class KeyboardInput:
     def __init__(self):
-        deault_setting_file_path = path_to_src + '/configs/settings.yaml'
+        deault_setting_file_path = path_to_src + "/configs/settings.yaml"
 
-        setting_file_path = rospy.get_param('setting_file_path')
-        if setting_file_path == '':
+        setting_file_path = rospy.get_param("setting_file_path")
+        if setting_file_path == "":
             setting_file_path = deault_setting_file_path
 
         self.robot = Robot(setting_file_path)
 
-        self.ee_vel_goals_pub = rospy.Publisher('relaxed_ik/ee_vel_goals', EEVelGoals, queue_size=5)
+        self.ee_vel_goals_pub = rospy.Publisher(
+            "relaxed_ik/ee_vel_goals", EEVelGoals, queue_size=5
+        )
 
-        self.pos_stride = 0.005
-        self.rot_stride = 0.010
+        self.pos_stride = 0.001
+        self.rot_stride = 0.005
 
         self.seq = 1
-
-        self.linear = [0,0,0]
-        self.angular = [0,0,0]
+
+        self.linear = np.zeros((self.robot.num_chain, 3))
+        self.angular = np.zeros((self.robot.num_chain, 3))
+        self.active_chain = 0
 
         keyboard_listener = keyboard.Listener(
-            on_press = self.on_press,
-            on_release = self.on_release)
+            on_press=self.on_press, on_release=self.on_release
+        )
 
         rospy.Timer(rospy.Duration(0.033), self.timer_callback)
 
         keyboard_listener.start()
 
     def on_press(self, key):
-        self.linear = [0,0,0]
-        self.angular = [0,0,0]
-
-        if key.char == 'w':
-            self.linear[0] += self.pos_stride
-        elif key.char == 'x':
-            self.linear[0] -= self.pos_stride
-        elif key.char == 'a':
-            self.linear[1] += self.pos_stride
-        elif key.char == 'd':
-            self.linear[1] -= self.pos_stride
-        elif key.char == 'q':
-            self.linear[2] += self.pos_stride
-        elif key.char == 'z':
-            self.linear[2] -= self.pos_stride
-        elif key.char == '1':
-            self.angular[0] += self.rot_stride
-        elif key.char == '2':
-            self.angular[0] -= self.rot_stride
-        elif key.char == '3':
-            self.angular[1] += self.rot_stride
-        elif key.char == '4':
-            self.angular[1] -= self.rot_stride
-        elif key.char == '5':
-            self.angular[2] += self.rot_stride
-        elif key.char == '6':
-            self.angular[2] -= self.rot_stride
-        elif key.char == 'c':
-            rospy.signal_shutdown()
-
-        print("   Linear Vel: {}, Angular Vel: {}".format(self.linear, self.angular))
+        self.linear = np.zeros((self.robot.num_chain, 3))
+        self.angular = np.zeros((self.robot.num_chain, 3))
+
+        if key.char.isnumeric():
+            assert int(key.char) <= self.robot.num_chain
+            self.active_chain = int(key.char) - 1
+        elif key.char == "w":
+            self.linear[self.active_chain][0] += self.pos_stride
+        elif key.char == "x":
+            self.linear[self.active_chain][0] -= self.pos_stride
+        elif key.char == "a":
+            self.linear[self.active_chain][1] += self.pos_stride
+        elif key.char == "d":
+            self.linear[self.active_chain][1] -= self.pos_stride
+        elif key.char == "e":
+            self.linear[self.active_chain][2] += self.pos_stride
+        elif key.char == "c":
+            self.linear[self.active_chain][2] -= self.pos_stride
+        elif key.char == "r":
+            self.angular[self.active_chain][0] += self.rot_stride
+        elif key.char == "v":
+            self.angular[self.active_chain][0] -= self.rot_stride
+        elif key.char == "d":
+            self.angular[self.active_chain][1] += self.rot_stride
+        elif key.char == "g":
+            self.angular[self.active_chain][1] -= self.rot_stride
+        elif key.char == "t":
+            self.angular[self.active_chain][2] += self.rot_stride
+        elif key.char == "b":
+            self.angular[self.active_chain][2] -= self.rot_stride
+
+        print(
+            f"Chain: {self.active_chain+1}, Linear Vel: {self.linear[self.active_chain]}, Angular Vel: {self.angular[self.active_chain]}"
+        )
 
     def on_release(self, key):
-        self.linear = [0,0,0]
-        self.angular = [0,0,0]
+        self.linear = np.zeros((self.robot.num_chain, 3))
+        self.angular = np.zeros((self.robot.num_chain, 3))
 
     def timer_callback(self, event):
         msg = EEVelGoals()
 
         for i in range(self.robot.num_chain):
             twist = Twist()
-            twist.linear.x = self.linear[0]
-            twist.linear.y = self.linear[1]
-            twist.linear.z = self.linear[2]
+            twist.linear.x = self.linear[i][0]
+            twist.linear.y = self.linear[i][1]
+            twist.linear.z = self.linear[i][2]
 
-            twist.angular.x = self.angular[0]
-            twist.angular.y = self.angular[1]
-            twist.angular.z = self.angular[2]
+            twist.angular.x = self.angular[i][0]
+            twist.angular.y = self.angular[i][1]
+            twist.angular.z = self.angular[i][2]
 
             tolerance = Twist()
             tolerance.linear.x = 0.0
@@ -103,10 +118,10 @@ def timer_callback(self, event):
 
         self.seq += 1
         self.ee_vel_goals_pub.publish(msg)
-        
-        
-if __name__ == '__main__':
-    rospy.init_node('keyboard_input')
+
+
+if __name__ == "__main__":
+    rospy.init_node("keyboard_input")
 
     keyboard = KeyboardInput()
-    rospy.spin()
+    rospy.spin()

scripts/robot.py

@@ -1,88 +1,118 @@
 #! /usr/bin/env python3
 
-import numpy as np
 import os
+from timeit import default_timer as timer
+
+import numpy as np
+import PyKDL
 import rospkg
 import rospy
 import transformations as T
-
+import yaml
 from geometry_msgs.msg import Pose, Vector3
-from std_msgs.msg import Float32MultiArray, Bool, String
+from kdl_parser import kdl_tree_from_urdf_model
 from sensor_msgs.msg import JointState
-from timeit import default_timer as timer
+from std_msgs.msg import Bool, Float32MultiArray, String
 from urdf_parser_py.urdf import URDF
-import PyKDL
-from kdl_parser import kdl_tree_from_urdf_model
-import yaml
 
-class Robot():
-    def __init__(self, setting_path = None):
-        path_to_src = rospkg.RosPack().get_path('relaxed_ik_ros1') + '/relaxed_ik_core'
-        setting_file_path = path_to_src + '/configs/settings.yaml'
-        if setting_path != '':
-           setting_file_path = setting_path
+
+class Robot:
+    def __init__(self, setting_path=None):
+        path_to_src = rospkg.RosPack().get_path("relaxed_ik_ros1") + "/relaxed_ik_core"
+        setting_file_path = path_to_src + "/configs/settings.yaml"
+        if setting_path != "":
+            setting_file_path = setting_path
         os.chdir(path_to_src)
 
         # Load the infomation
-        setting_file = open(setting_file_path, 'r')
+        setting_file = open(setting_file_path, "r")
         settings = yaml.load(setting_file, Loader=yaml.FullLoader)
 
         urdf_name = settings["urdf"]
-        
+
         self.robot = URDF.from_xml_file(path_to_src + "/configs/urdfs/" + urdf_name)
         self.kdl_tree = kdl_tree_from_urdf_model(self.robot)
-        
-        # all non-fixed joint         
+
+        # all non-fixed joint
         self.joint_lower_limits = []
         self.joint_upper_limits = []
         self.joint_vel_limits = []
         self.all_joint_names = []
         for j in self.robot.joints:
-            if j.type != 'fixed':
+            if j.type != "fixed":
                 self.joint_lower_limits.append(j.limit.lower)
                 self.joint_upper_limits.append(j.limit.upper)
                 self.joint_vel_limits.append(j.limit.velocity)
                 self.all_joint_names.append(j.name)
 
         # joints solved by relaxed ik
         self.articulated_joint_names = []
-        assert len(settings['base_links']) == len(settings['ee_links']) 
-        self.num_chain = len(settings['base_links'])
+        assert len(settings["base_links"]) == len(settings["ee_links"])
+        self.num_chain = len(settings["base_links"])
 
         for i in range(self.num_chain):
-            arm_chain = self.kdl_tree.getChain( settings['base_links'][i],
-                                                settings['ee_links'][i])
+            arm_chain = self.kdl_tree.getChain(
+                settings["base_links"][i], settings["ee_links"][i]
+            )
             for j in range(arm_chain.getNrOfSegments()):
                 joint = arm_chain.getSegment(j).getJoint()
-                # 8 is fixed joint   
-                if joint.getType() != 8:
+                # 8 is fixed joint
+                if (
+                    joint.getType() != 8
+                    and joint.getName() not in self.articulated_joint_names
+                ):
                     self.articulated_joint_names.append(joint.getName())
 
-        if 'starting_config' in settings:
-            assert len(self.articulated_joint_names) == len(settings['starting_config']), \
-                        "Number of joints parsed from urdf should be the same with the starting config in the setting file."
+        if "joint_ordering" in settings and "starting_config" in settings:
+            assert len(settings["joint_ordering"]) == len(
+                settings["starting_config"]
+            ), "Number of joints parsed from urdf should be the same with the starting config in the setting file."
 
         self.arm_chains = []
+        self.chain_indices = []
         self.fk_p_kdls = []
         self.fk_v_kdls = []
         self.ik_p_kdls = []
         self.ik_v_kdls = []
-        self.num_jnts = []
+        # self.num_jnts = []
         for i in range(self.num_chain):
-            arm_chain = self.kdl_tree.getChain( settings['base_links'][i],
-                                                settings['ee_links'][i])
+            arm_chain = self.kdl_tree.getChain(
+                settings["base_links"][i], settings["ee_links"][i]
+            )
             self.arm_chains.append(arm_chain)
             self.fk_p_kdls.append(PyKDL.ChainFkSolverPos_recursive(arm_chain))
             self.fk_v_kdls.append(PyKDL.ChainFkSolverVel_recursive(arm_chain))
             self.ik_v_kdls.append(PyKDL.ChainIkSolverVel_pinv(arm_chain))
-            self.ik_p_kdls.append(PyKDL.ChainIkSolverPos_NR(arm_chain, 
-                                                            self.fk_p_kdls[i],
-                                                            self.ik_v_kdls[i]))
-            self.num_jnts.append(arm_chain.getNrOfJoints())
+            self.ik_p_kdls.append(
+                PyKDL.ChainIkSolverPos_NR(
+                    arm_chain, self.fk_p_kdls[i], self.ik_v_kdls[i]
+                )
+            )
+            joint_indices = []
+            articulated_joint_index = 0
+            for j in range(arm_chain.getNrOfSegments()):
+                joint = arm_chain.getSegment(j).getJoint()
+                if "joint_ordering" not in settings:
+                    if joint.getType() != 8:
+                        joint_indices.append(articulated_joint_index)
+                        articulated_joint_index += 1
+                else:
+                    try:
+                        joint_index = settings["joint_ordering"].index(joint.getName())
+                        joint_indices.append(joint_index)
+                    except ValueError:
+                        pass
+
+            self.chain_indices.append(joint_indices)
+            # self.num_jnts.append(arm_chain.getNrOfJoints())
 
     def fk_single_chain(self, fk_p_kdl, joint_angles, num_jnts):
-        assert len(joint_angles) == num_jnts, "length of input: {}, number of joints: {}".format(len(joint_angles), num_jnts)
-
+        assert (
+            len(joint_angles) == num_jnts
+        ), "length of input: {}, number of joints: {}".format(
+            len(joint_angles), num_jnts
+        )
+
         kdl_array = PyKDL.JntArray(num_jnts)
         for idx in range(num_jnts):
             kdl_array[idx] = joint_angles[idx]
@@ -104,20 +134,21 @@ def fk_single_chain(self, fk_p_kdl, joint_angles, num_jnts):
         return pose
 
     def fk(self, joint_angles):
-        l = 0
-        r = 0
         poses = []
         for i in range(self.num_chain):
-            r += self.num_jnts[i]
-            pose = self.fk_single_chain(self.fk_p_kdls[i], joint_angles[l:r], self.num_jnts[i])
-            l = r
+            joint_values = [joint_angles[x] for x in self.chain_indices[i]]
+            pose = self.fk_single_chain(
+                self.fk_p_kdls[i],
+                joint_values,
+                len(self.chain_indices[i]),
+            )
             poses.append(pose)
-        
+
         return poses
 
     def get_joint_state_msg(self, joint_angles):
         js = JointState()
         js.header.stamp = rospy.Time.now()
         js.name = self._joint_names
         js.position = joint_angles
-        return js
+        return js