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__':
