Skip to content

GitLab

Commit 3d4b2d9e authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

inverse kinematics service working

parent b576fac9
Hide whitespace changes
Inline Side-by-side
controller.py
View file @ 3d4b2d9e
......@@ -101,6 +101,7 @@ class controller:
def fk(self, joints):
"""
compute forward kinematics
This uses the /compute_fk rosservice - which doesnt work
"""
rospy.wait_for_service('compute_fk')
......@@ -110,7 +111,7 @@ class controller:
rospy.logerr("Service call failed: %s"%e)
req = GetPositionFKRequest()
req.header.frame_id = 'base_link'
req.header.frame_id = 'base'
req.fk_link_names = ['ee_link']
req.robot_state.joint_state.name = self.group.get_joints()
req.robot_state.joint_state.position = joints
......@@ -118,19 +119,31 @@ class controller:
return moveitFK.call(req)
def ik(self, pose):
def ik(self, pose, start_angle = None):
"""
Inverse Kinematics
Using /compute_ik rosservice
- this one actually works
"""
if start_angle = None:
start_angle = con.group.get_current_joint_values()
rospy.wait_for_service('compute_ik')
try:
moveitIK = rospy.ServiceProxy('compute_ik', GetPositionFK)
moveitIK = rospy.ServiceProxy('compute_ik', GetPositionIK)
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s"%e)
req = GetPositionIKRequest()
req.ik_request.ik_link_name = 'ee_link'
req.ik_request.group_name = 'manipulator'
req.ik_request.robot_state.joint_state.name = con.group.get_joints()
req.ik_request.robot_state.joint_state.position = start_angle
req.ik_request.pose_stamped = pose
return moveitIK.call(req)
pass
def exec_traj(self, traj):
"""
......@@ -220,17 +233,17 @@ class controller:
"""
if key == keyboard.Key.up:
self._internal_move_endpoint( 0.1, 0, 0)
self._internal_move_endpoint( 0.01, 0, 0)
elif key == keyboard.Key.down:
self._internal_move_endpoint(-0.1, 0, 0)
self._internal_move_endpoint(-0.01, 0, 0)
elif key == keyboard.Key.left:
self._internal_move_endpoint(0, 0.1, 0)
self._internal_move_endpoint(0, 0.01, 0)
elif key == keyboard.Key.right:
self._internal_move_endpoint(0, -0.1, 0)
self._internal_move_endpoint(0, -0.01, 0)
elif key == keyboard.Key.page_up:
self._internal_move_endpoint(0, 0, 0.1)
self._internal_move_endpoint(0, 0, 0.01)
elif key == keyboard.Key.page_down:
self._internal_move_endpoint(0, 0, -0.1)
self._internal_move_endpoint(0, 0, -0.01)
elif key == keyboard.Key.enter:
# save current pose into calibaration
return self._temp_calib.push_parameter(self.get_joints())
......
utils/__init__.py deleted 100644 → 0
View file @ b576fac9
from ur_kinematics import ur_kinematics
utils/ur_kinematics.py deleted 100644 → 0
View file @ b576fac9
#!/usr/bin/python2
import numpy as np
from numpy import linalg
import cmath
import math
from math import cos,sin,atan2,acos,asin,sqrt,pi
from numpy import matrix as mat
# inverse Kinematics for UR robots (UR3/UR5/UR10)
# adapted from https://github.com/mc-capolei/python-Universal-robot-kinematics
# input robot must be "UR10", "UR5" or "UR3"
class ur_kinematics:
"""
UR Kinematics
When Creating instance of class, input must be 'UR10', 'UR5', or 'UR3'
does forward kinematics using function fk([0,0,0,0,0,0]) #input is a list of 6 joint angles
and inverse kinematics using function ik(T) # where T is the desired transformation matrix
NOTE: It does not take into account any attached end effectors, this functionality need to
be added by storing an end-effector transformation matrix
"""
def __init__(self, robot):
if robot == "UR10":
#Set Dimension Parameters
self.d = [0.1273, 0, 0, 0.163941, 0.1157, 0.0922]
self.a = [0 ,-0.612, -0.5723, 0, 0, 0]
self.alph = [pi/2, 0, 0, pi/2, -pi/2, 0]
elif robot == "UR5":
#Set Dimension Parameters
self.d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0823]
self.a = [0 ,-0.425, -0.39225, 0, 0, 0]
self.alph = [pi/2, 0, 0, pi/2, -pi/2, 0]
elif robot == "UR3":
#Set Dimension Parameters
self.d = [0.1519, 0, 0, 0.11235, 0.08535, 0.0819]
self.a = [0 ,-0.24365, -0.21325, 0, 0, 0]
self.alph = [pi/2, 0, 0, pi/2, -pi/2, 0]
elif robot == "UR16e":
#Set Dimension Parameters
self.d = [0.1807, 0, 0, 0.17415, 0.11985, 0.11655]
self.a = [0 ,-0.4784, -0.36, 0, 0, 0]
self.alph = [pi/2, 0, 0, pi/2, -pi/2, 0]
elif robot == "UR10e":
#Set Dimension Parameters
self.d = [0.1807, 0, 0, 0.17415, 0.11985, 0.11655]
self.a = [0 ,-0.6127, -0.57155, 0, 0, 0]
self.alph = [pi/2, 0, 0, pi/2, -pi/2, 0]
elif robot == "UR5e":
#Set Dimension Parameters
self.d = [0.1625, 0, 0, 0.1333, 0.0997, 0.0996]
self.a = [0 ,-0.425, -0.3922, 0, 0, 0]
self.alph = [pi/2, 0, 0, pi/2, -pi/2, 0]
elif robot == "UR3e":
#Set Dimension Parameters
self.d = [0.15185, 0, 0, 0.13105, 0.08535, 0.0921]
self.a = [0 ,-0.24355, -0.2132, 0, 0, 0]
self.alph = [pi/2, 0, 0, pi/2, -pi/2, 0]
else:
print "Invalid robot selection, must be one of 'UR10', 'UR5', 'UR3', 'UR16e', 'UR10e', 'UR5e' or 'UR3e'"
# forward kinematics along one joint
def _ah(self, n, th, c=0):
th = th[n-1, c]
al = self.alph[n-1]
T_a = mat(np.identity(4), copy=False)
T_a[0,3] = self.a[n-1]
T_d = mat(np.identity(4), copy=False)
T_d[2,3] = self.d[n-1]
Rzt = mat([[cos(th), -sin(th), 0, 0],
[sin(th), cos(th), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
Rxa = mat([[1,0,0,0],
[0, cos(al), -sin(al), 0],
[0, sin(al), cos(al), 0],
[0,0,0,1]])
return T_d * Rzt * T_a *Rxa
# Forward kinematics - simply multiplying fk transformation matrices along the arm
def fk(self, joints):
if isinstance(joints, list):
joints = mat(joints).T
return self._ah(1, joints) * self._ah(2, joints) * self._ah(3, joints) * \
self._ah(4, joints) * self._ah(5, joints) * self._ah(6, joints)
# Inverse Kinematics
def ik(self, Tf):
th = mat(np.zeros((6,8))) # there are only 8 solutions
P_05 = (Tf * mat([0,0,self.d[5],1]).T - mat([0,0,0,1]).T)
##### theta 1
psi = atan2(P_05[2-1,0], P_05[1-1,0])
phi = acos(self.d[3] /sqrt(P_05[2-1,0]*P_05[2-1,0] + P_05[1-1,0]*P_05[1-1,0]))
# 2 solutions corresponding to theta 1
th[0, 0:4] = pi/2 + psi + phi
th[0, 4:8] = pi/2 + psi - phi
th = th.real
##### theta 5
for c in [0,4]:
T_10 = linalg.inv(self._ah(1, th, c))
T_16 = T_10 * Tf
th[4, c:c+2] = acos((T_16[2,3]-self.d[3])/self.d[5])
th[4, c+2:c+4] = -acos((T_16[2,3]-self.d[3])/self.d[5])
th = th.real
##### theta 6
# Note: not well defined at sin(theta5)=0 or when T16(1,3), T16(2,3) =0
for c in [0,2,4,6]:
T_10 = linalg.inv(self._ah(1, th, c))
T_16 = linalg.inv(T_10 * Tf)
th[5, c:c+2] = atan2((-T_16[1,2]/sin(th[4, c])),(T_16[0,2]/sin(th[4, c])))
th = th.real
##### theta 3
for c in [0,2,4,6]:
T_10 = linalg.inv(self._ah(1, th, c))
T_65 = self._ah(6, th, c)
T_54 = self._ah(5, th, c)
T_14 = (T_10 * Tf) * linalg.inv(T_54 * T_65)
P_13 = T_14 * mat([0,-self.d[3], 0, 1]).T - mat([0,0,0,1]).T
t3 = cmath.acos((linalg.norm(P_13)**2 - self.a[1]**2 - self.a[2]**2)/(2 * self.a[1] * self.a[2]))
th[2,c] = t3.real
th[2,c+1] = -t3.real
##### theta 2 & 4
for c in range(8):
T_10 = linalg.inv(self._ah(1, th, c))
T_65 = linalg.inv(self._ah(6, th, c))
T_54 = linalg.inv(self._ah(5, th, c))
T_14 = (T_10 * Tf) * T_65 * T_54
P_13 = T_14 * mat([0, -self.d[3], 0, 1]).T - mat([0,0,0,1]).T
# theta 2
th[1, c] = -atan2(P_13[1], -P_13[0]) + asin(self.a[2]* sin(th[2,c])/linalg.norm(P_13))
# theta 4
T_32 = linalg.inv(self._ah(3, th, c))
T_21 = linalg.inv(self._ah(2, th, c))
T_34 = T_32 * T_21 * T_14
th[3,c] = atan2(T_34[1,0], T_34[0,0])
th = th.real.T.tolist()
#Greater than 1 cm discrepency between true and desired => no solution
if (Tf - self.fk(th[0])).sum()>0.05:
th = []
print "IK not possible"
# Reset all angles to be between -pi and +pi
for j in range(len(th)):
for a in range(len(th[j])):
if th[j][a] < -pi:
th[j][a] += 2*pi
elif th[j][a] > pi:
th[j][a] -= 2*pi
return th
def get_closest(self, js, j):
"""
Find the closest joint seq in js to joint_angles j
js is list of lists
j is list
"""
diff = []
print js
print j
if len(js)==0:
print "js must be a list of lists"
for i in js:
diff.append(max([abs(i[a]-j[a]) for a in range(len(j))]))
best = diff.index(min(diff))
return js[best], diff[best]
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment