diff --git a/.gitmodules b/.gitmodules index afa26ac..84237f1 100644 --- a/.gitmodules +++ b/.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 diff --git a/relaxed_ik_core b/relaxed_ik_core index c15d69e..913866a 160000 --- a/relaxed_ik_core +++ b/relaxed_ik_core @@ -1 +1 @@ -Subproject commit c15d69eafb96e0de9b230381a6fb146f51fc1ffb +Subproject commit 913866aabd1e304e30c6c971321ee13318b778d2 diff --git a/scripts/keyboard_input.py b/scripts/keyboard_input.py index 8d21c92..42e8183 100755 --- a/scripts/keyboard_input.py +++ b/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() \ No newline at end of file + rospy.spin() diff --git a/scripts/robot.py b/scripts/robot.py index e746f63..b9ddabf 100644 --- a/scripts/robot.py +++ b/scripts/robot.py @@ -1,44 +1,45 @@ #! /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) @@ -46,43 +47,72 @@ def __init__(self, setting_path = None): # 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,15 +134,16 @@ 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): @@ -120,4 +151,4 @@ def get_joint_state_msg(self, joint_angles): js.header.stamp = rospy.Time.now() js.name = self._joint_names js.position = joint_angles - return js \ No newline at end of file + return js