def __init__(self, robot, base_link=None, tip_link=None, data_path="../.."): cur_path = os.path.dirname(os.path.abspath(__file__)) if PYTHON2: import cPickle as pickle with open(cur_path + "/robot.pkl", "rb") as fid: robot_info = pickle.load(fid) else: import pickle with open(cur_path + "/robot_p3.pkl", "rb") as fid: robot_info = pickle.load(fid) self._pose_0 = robot_info["_pose_0"] self.finger_pose = self._pose_0[-2].copy() self._joint_origin = robot_info["_joint_axis"] self._tip2joint = robot_info["_tip2joint"] self._joint_axis = robot_info["_joint_axis"] self._joint_limits = robot_info["_joint_limits"] self._joint2tips = robot_info["_joint2tips"] self._joint_name = robot_info["_joint_name"] self.center_offset = np.array(robot_info["center_offset"]) self._link_names = robot_info["_link_names"] self._base_link, self._tip_link = "panda_link0", "panda_hand" self._num_jnts = 7 self._robot = URDF.from_xml_string( open( os.path.join(cur_path, data_path, "data/robots", "panda_arm_hand.urdf"), "r", ).read()) self._kdl_tree, _ = kdl_tree_from_urdf_model(self._robot) self.soft_joint_limit_padding = 0.2 mins_kdl = joint_list_to_kdl( np.array([ self._joint_limits[n][0] + self.soft_joint_limit_padding for n in self._joint_name[:-3] ])) maxs_kdl = joint_list_to_kdl( np.array([ self._joint_limits[n][1] - self.soft_joint_limit_padding for n in self._joint_name[:-3] ])) self._arm_chain = self._kdl_tree.getChain(self._base_link, self._tip_link) self._fk_p_kdl = PyKDL.ChainFkSolverPos_recursive(self._arm_chain) self._ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) self._ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl, maxs_kdl, self._fk_p_kdl, self._ik_v_kdl)
def inverse(self, pose, q_guess=None, min_joints=None, max_joints=None): pos, rot = PoseConv.to_pos_rot(pose) pos_kdl = kdl.Vector(pos[0, 0], pos[1, 0], pos[2, 0]) rot_kdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0], rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1], rot[2, 2]) frame_kdl = kdl.Frame(rot_kdl, pos_kdl) if min_joints is None: min_joints = self.joint_safety_lower if max_joints is None: max_joints = self.joint_safety_upper mins_kdl = joint_list_to_kdl(min_joints) maxs_kdl = joint_list_to_kdl(max_joints) ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(self.chain, mins_kdl, maxs_kdl, self._fk_kdl, self._ik_v_kdl) if q_guess == None: # use the midpoint of the joint limits as the guess lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.) upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.) q_guess = (lower_lim + upper_lim) / 2.0 q_guess = np.where(np.isnan(q_guess), [0.] * len(q_guess), q_guess) q_kdl = kdl.JntArray(self.num_joints) q_guess_kdl = joint_list_to_kdl(q_guess) if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0: return np.array(joint_kdl_to_list(q_kdl)) else: return None
def kdl_inverse_kinematics_jl2(self, pos, quat, seed=None, min_joints=None, max_joints=None): pos = PyKDL.Vector(pos[0], pos[1], pos[2]) rot = PyKDL.Rotation() rot = rot.Quaternion(quat[0], quat[1], quat[2], quat[3]) goal_pose = PyKDL.Frame(rot, pos) if min_joints is None: min_joints = self.joint_limits_lower if max_joints is None: max_joints = self.joint_limits_upper mins_kdl = joint_list_to_kdl(min_joints) maxs_kdl = joint_list_to_kdl(max_joints) ik_jl_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl, maxs_kdl, self._fk_p_kdl, self._ik_v_kdl) seed_array = joint_list_to_kdl(self._default_seed) if seed != None: seed_array = joint_list_to_kdl(seed) result_angles = PyKDL.JntArray(self._num_jnts) if ik_jl_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(list(result_angles)) # return result return result else: # return None return None
def __inverse_kinematics(self, position, orientation=None, seed=None): """inverse kinematic solver using PyKDL""" _inverse_kin_position_kdl = PyKDL.ChainIkSolverPos_NR_JL(self.iiwa_chain, self.min_limits, self.max_limits, self.forward_kin_position_kdl, self.inverse_kin_velocity_kdl) ik = PyKDL.ChainIkSolverVel_pinv(self.iiwa_chain) pos = PyKDL.Vector(position[0], position[1], position[2]) if orientation != None: rot = PyKDL.Rotation() #PyKDL uses w, x, y, z instead of x, y, z, w rot = rot.Quaternion(orientation[1], orientation[2], orientation[3], orientation[0]) seed_array = PyKDL.JntArray(NUM_JOINTS) if seed != None: seed_array.resize(len(seed)) for idx, jnt in enumerate(seed): seed_array[idx] = jnt else: joint_vals = self.group.get_current_joint_values() for idx, jnt in enumerate(joint_vals): seed_array[idx] = joint_vals[idx] if orientation: goal_pose = PyKDL.Frame(rot, pos) else: goal_pose = PyKDL.Frame(pos) result_angles = PyKDL.JntArray(NUM_JOINTS) if _inverse_kin_position_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(result_angles) return result else: print 'No IK Solution Found' return None
def get_kinematics_solvers(kdl_chain, kdl_limits_min, kdl_limits_max): fk_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain) velocity_ik = PyKDL.ChainIkSolverVel_pinv(kdl_chain) # ik_solver = PyKDL.ChainIkSolverPos_LMA(kdl_chain, 1e-5, 1000, 1e-15) ik_solver = PyKDL.ChainIkSolverPos_NR_JL(kdl_chain, kdl_limits_min, kdl_limits_max, fk_solver, velocity_ik) return fk_solver, ik_solver
def calculate_ik(base_link, tip_link, seed_joint_state, goal_transform_geometry_msg): """ Calculates the Inverse Kinematics from base_link to tip_link according to the given goal_transform_geometry_msg. The initial joint states would be considered from seed_joint_state. Returns the result joint states and the success status. base_link eg. - "triangle_base_link" or "calib_left_arm_base_link" or "calib_right_arm_base_link" tip_link eg. - "left_arm_7_link" or "right_arm_7_link" """ robot_urdf_string = rospy.get_param('robot_description') urdf_obj = urdf_parser_py.urdf.URDF.from_xml_string(robot_urdf_string) _, kdl_tree = kdl_parser_py.urdf.treeFromUrdfModel(urdf_obj) kdl_chain = kdl_tree.getChain(base_link, tip_link) num_joints = kdl_chain.getNrOfJoints() print "number of joints: " + str(num_joints) # Get Joint limits kdl_joint_limits_min, kdl_joint_limits_max = get_kdl_joint_limit_arrays( kdl_chain, urdf_obj) fk_solver = PyKDL.ChainFkSolverPos_recursive(kdl_chain) velocity_ik = PyKDL.ChainIkSolverVel_pinv(kdl_chain) # ik_solver = PyKDL.ChainIkSolverPos_LMA(kdl_chain, 1e-5, 1000, 1e-15) ik_solver = PyKDL.ChainIkSolverPos_NR_JL(kdl_chain, kdl_joint_limits_min, kdl_joint_limits_max, fk_solver, velocity_ik) # Getting the goal frame and seed state goal_frame_kdl = tf2_kdl.transform_to_kdl(goal_transform_geometry_msg) seed_joint_state_kdl = get_kdl_jnt_array_from_list(num_joints, seed_joint_state) # Solving IK result_joint_state_kdl = solve_ik(ik_solver, num_joints, seed_joint_state_kdl, goal_frame_kdl) # check if calculated joint state results in the correct end-effector position using FK goal_pose_reached = check_ik_result_using_fk(fk_solver, result_joint_state_kdl, goal_frame_kdl) # check if calculated joint state is within joint limits joints_within_limits = check_result_joints_are_within_limits( num_joints, result_joint_state_kdl, kdl_joint_limits_min, kdl_joint_limits_max) print "Result Joint State Within Limits: " + str(joints_within_limits) print "Can Reach Goal Pose With Solution: " + str(goal_pose_reached) result_joint_state_vector = get_list_from_kdl_jnt_array( num_joints, result_joint_state_kdl) goal_pose_reached_successfully = goal_pose_reached and joints_within_limits return result_joint_state_vector, goal_pose_reached_successfully
def inverse_kinematics(robot_chain, pos, rot, q_guess=None, min_joints=None, max_joints=None): """ Perform inverse kinematics Args: robot_chain: robot's chain object pos: 1 x 3 or 3 x 1 array of the end effector position. rot: 3 x 3 array of the end effector rotation q_guess: guess values for the joint positions min_joints: minimum value of the position for each joint max_joints: maximum value of the position for each joint Returns: list of joint positions or None (no solution) """ pos_kdl = kdl.Vector(pos[0], pos[1], pos[2]) rot_kdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0], rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1], rot[2, 2]) frame_kdl = kdl.Frame(rot_kdl, pos_kdl) num_joints = robot_chain.getNrOfJoints() min_joints = -np.pi * np.ones( num_joints) if min_joints is None else min_joints max_joints = np.pi * np.ones( num_joints) if max_joints is None else max_joints mins_kdl = joint_list_to_kdl(min_joints) maxs_kdl = joint_list_to_kdl(max_joints) fk_kdl = kdl.ChainFkSolverPos_recursive(robot_chain) ik_v_kdl = kdl.ChainIkSolverVel_pinv(robot_chain) ik_p_kdl = kdl.ChainIkSolverPos_NR_JL(robot_chain, mins_kdl, maxs_kdl, fk_kdl, ik_v_kdl) if q_guess == None: # use the midpoint of the joint limits as the guess lower_lim = np.where(np.isfinite(min_joints), min_joints, 0.) upper_lim = np.where(np.isfinite(max_joints), max_joints, 0.) q_guess = (lower_lim + upper_lim) / 2.0 q_guess = np.where(np.isnan(q_guess), [0.] * len(q_guess), q_guess) q_kdl = kdl.JntArray(num_joints) q_guess_kdl = joint_list_to_kdl(q_guess) if ik_p_kdl.CartToJnt(q_guess_kdl, frame_kdl, q_kdl) >= 0: return joint_kdl_to_list(q_kdl) else: return None
def inverse_kinematics(self, position, orientation=None, seed=None, min_joints=None, max_joints=None, maxiter=500, eps=1.0e-6): ik = PyKDL.ChainIkSolverVel_pinv(self._arm_chain) pos = PyKDL.Vector(position[0], position[1], position[2]) if orientation is not None: rot = PyKDL.Rotation() rot = rot.Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]) # Populate seed with current angles if not provided seed_array = PyKDL.JntArray(self._num_jnts) if seed is not None: seed_array.resize(len(seed)) for idx, jnt in enumerate(seed): seed_array[idx] = jnt else: seed_array = self.joints_to_kdl('positions') # Make IK Call if orientation is not None: goal_pose = PyKDL.Frame(rot, pos) else: goal_pose = PyKDL.Frame(pos) result_angles = PyKDL.JntArray(self._num_jnts) # Make IK solver with joint limits if min_joints is None: min_joints = self.joint_limits_lower if max_joints is None: max_joints = self.joint_limits_upper mins_kdl = PyKDL.JntArray(len(min_joints)) for idx, jnt in enumerate(min_joints): mins_kdl[idx] = jnt maxs_kdl = PyKDL.JntArray(len(max_joints)) for idx, jnt in enumerate(max_joints): maxs_kdl[idx] = jnt ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(self._arm_chain, mins_kdl, maxs_kdl, self._fk_p_kdl, self._ik_v_kdl, maxiter, eps) if ik_p_kdl.CartToJnt(seed_array, goal_pose, result_angles) >= 0: result = np.array(list(result_angles)) return result else: return None
def inverseKinematics(robotChain, pos, rot, qGuess=None, minJoints=None, maxJoints=None): """ Perform inverse kinematics Args: robotChain: robot's chain object pos: 1 x 3 or 3 x 1 array of the end effector position. rot: 3 x 3 array of the end effector rotation qGuess: guess values for the joint positions minJoints: minimum value of the position for each joint maxJoints: maximum value of the position for each joint Returns: list of joint positions or None (no solution) """ # print("inside inverse: ", pos, " ; ", rot) posKdl = kdl.Vector(pos[0], pos[1], pos[2]) rotKdl = kdl.Rotation(rot[0, 0], rot[0, 1], rot[0, 2], rot[1, 0], rot[1, 1], rot[1, 2], rot[2, 0], rot[2, 1], rot[2, 2]) frameKdl = kdl.Frame(rotKdl, posKdl) numJoints = robotChain.getNrOfJoints() minJoints = -np.pi * np.ones(numJoints) if minJoints is None else minJoints maxJoints = np.pi * np.ones(numJoints) if maxJoints is None else maxJoints minsKdl = jointListToKdl(minJoints) maxsKdl = jointListToKdl(maxJoints) fkKdl = kdl.ChainFkSolverPos_recursive(robotChain) ikVKdl = kdl.ChainIkSolverVel_pinv(robotChain) ikPKdl = kdl.ChainIkSolverPos_NR_JL(robotChain, minsKdl, maxsKdl, fkKdl, ikVKdl) if qGuess is None: # use the midpoint of the joint limits as the guess lowerLim = np.where(np.isfinite(minJoints), minJoints, 0.) upperLim = np.where(np.isfinite(maxJoints), maxJoints, 0.) qGuess = (lowerLim + upperLim) / 2.0 qGuess = np.where(np.isnan(qGuess), [0.]*len(qGuess), qGuess) qKdl = kdl.JntArray(numJoints) qGuessKdl = jointListToKdl(qGuess) if ikPKdl.CartToJnt(qGuessKdl, frameKdl, qKdl) >= 0: return jointKdlToList(qKdl) else: return None
def __init__(self, T=20, weight=[1.0,1.0]): #initialize model self.chain = PyKDL.Chain() self.chain.addSegment(PyKDL.Segment("Base", PyKDL.Joint(PyKDL.Joint.None), PyKDL.Frame(PyKDL.Vector(0.0, 0.0, 0.042)), PyKDL.RigidBodyInertia(0.08659, PyKDL.Vector(0.00025, 0.0, -0.02420), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint1", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.0, -0.019, 0.028)), PyKDL.RigidBodyInertia(0.00795, PyKDL.Vector(0.0, 0.019, -0.02025), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint2", PyKDL.Joint(PyKDL.Joint.RotY), PyKDL.Frame(PyKDL.Vector(0.0, 0.019, 0.0405)), PyKDL.RigidBodyInertia(0.09312, PyKDL.Vector(0.00000, -0.00057, -0.02731), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint3", PyKDL.Joint(PyKDL.Joint.RotZ), PyKDL.Frame(PyKDL.Vector(0.024, -0.019, 0.064)), PyKDL.RigidBodyInertia(0.19398, PyKDL.Vector(-0.02376, 0.01864, -0.02267), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint4", PyKDL.Joint("minus_RotY", PyKDL.Vector(0,0,0), PyKDL.Vector(0,-1,0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.024)), PyKDL.RigidBodyInertia(0.09824, PyKDL.Vector(-0.02099, 0.0, -0.01213), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint5", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.0405, -0.019, 0.0)), PyKDL.RigidBodyInertia(0.09312, PyKDL.Vector(-0.01319, 0.01843, 0.0), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint6", PyKDL.Joint("minus_RotY", PyKDL.Vector(0,0,0), PyKDL.Vector(0,-1,0), PyKDL.Joint.RotAxis), PyKDL.Frame(PyKDL.Vector(0.064, 0.019, 0.0)), PyKDL.RigidBodyInertia(0.09824, PyKDL.Vector(-0.02099, 0.0, 0.01142), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.chain.addSegment(PyKDL.Segment("Joint7", PyKDL.Joint(PyKDL.Joint.RotX), PyKDL.Frame(PyKDL.Vector(0.14103, 0.0, 0.0)), PyKDL.RigidBodyInertia(0.26121, PyKDL.Vector(-0.09906, 0.00146, -0.00021), PyKDL.RotationalInertia(1.0, 1.0, 1.0, 0.0, 0.0, 0.0)))) self.min_position_limit = [-160.0, -90.0, -160.0, -90.0, -160.0, -90.0, -160.0] self.max_position_limit = [160.0, 90.0, 160.0, 90.0, 160.0, 90.0, 160.0] self.min_joint_position_limit = PyKDL.JntArray(7) self.max_joint_position_limit = PyKDL.JntArray(7) for i in range (0,7): self.min_joint_position_limit[i] = self.min_position_limit[i]*pi/180 self.max_joint_position_limit[i] = self.max_position_limit[i]*pi/180 self.fksolver = PyKDL.ChainFkSolverPos_recursive(self.chain) self.iksolver = PyKDL.ChainIkSolverVel_pinv(self.chain) self.iksolverpos = PyKDL.ChainIkSolverPos_NR_JL(self.chain, self.min_joint_position_limit, self.max_joint_position_limit, self.fksolver, self.iksolver, 100, 1e-6) #parameter self.T = T self.weight_u = weight[0] self.weight_x = weight[1] self.nj = self.chain.getNrOfJoints() self.q_init = np.zeros(self.nj) self.q_out = np.zeros(self.nj) ret, self.dest, self.q_out = self.generate_dest() self.fin_position = self.dest.p return
def make_segment(joint): jt = PyKDL.Joint(joint['joint'], joint['origin'], joint['RotAxis'], PyKDL.Joint.RotAxis) seg = PyKDL.Segment(joint['segment'], jt, PyKDL.Frame(joint['origin'])) return seg chain = PyKDL.Chain() for joint in joints: chain.addSegment(make_segment(joint)) fk_kdl = PyKDL.ChainFkSolverPos_recursive(chain) ik_v_kdl = PyKDL.ChainIkSolverVel_pinv(chain) ik_p_kdl = PyKDL.ChainIkSolverPos_NR_JL(chain, q_lower, q_upper, fk_kdl, ik_v_kdl) def fk(q): frame = PyKDL.Frame() fk_kdl.JntToCart(q, frame) return frame def ik(frame, q_guess): q_res = PyKDL.JntArray(6) ik_p_kdl.CartToJnt(q_guess, frame, q_res) return q_res if __name__ == '__main__':