Commit 0e9e6c12 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

ur kinematics added, calibration code structure done

parent e5f26ae9
......@@ -3,9 +3,11 @@
Take a standard gcode file and print it using only 1 arm.
Includes the following important components:
- connecting with single arm through ros: todo
- connecting with extruder through ros: todo
- connecting with single arm through ros
- calibration: todo
- connecting with extruder through ros: todo
- printing the whole thing: todo
Required:
- pynput
- gcode2contour
from copy import copy, deepcopy
from pynput import keyboard
from enum import Enum
import time
import warnings
import rospy
from trajectory_msgs.msg import (
JointTrajectoryPoint,
JointTrajectory
)
from sensor_msgs.msg import JointState
import time
from copy import copy, deepcopy
class CalibrationType(Enum):
centre = 1
tiny_grid = 4
small_grid = 9
medium_grid = 16
large_grid = 25
class Calibration:
"""
Stores calibration parameters
"""
def __init__(self, calibration_type):
assert(isinstance(calibration_type, CalibrationType))
self.cb_t = calibration_type
self.cb_parameters = []
self.complete = False
def push_parameter(self, param):
"""
Save the joint angles to reach particular point in grid
param should be a list of floats (joint angles) (not asserted)
"""
if len(self.cb_parameters) == self.cb_t.value:
warnings.warn("all parameters are already set, ignoring this input")
return
self.cb_parameters.append(param)
if len(self.cb_parameters) == self.cb_t.value:
print "Calibration Complete"
class controller:
......@@ -17,6 +58,8 @@ class controller:
self.node = rospy.init_node("py_controller", anonymous = False)
self.traj = rospy.Publisher("scaled_pos_traj_controller/command", JointTrajectory, queue_size=1)
self.joint_state = None
self.calibration = None
self._temp_calib = None
time.sleep(1)
......@@ -51,8 +94,59 @@ class controller:
return t
def _internal_on_press(self, key):
"""
CALIBRATION component:
When a key is pressed during calibration, this is triggered
and takes various actions depending on the key
"""
if key == keyboard.Key.up:
pass
elif key == keyboard.Key.down:
pass
elif key == keyboard.Key.left:
pass
elif key == keyboard.Key.right:
pass
elif key == keyboard.Key.page_up:
pass
elif key == keyboard.Key.page_down:
pass
elif key == keyboard.Key.enter:
# save current pose into calibaration
self._temp_calib.push_parameter(self.get_joints())
def _internal_on_release(key):
"""
Stop calibration if escape key pressed
"""
if key == keyboard.Key.esc:
return False
def calibrate(self, cb_type):
"""
Calibrate robot by moving endpoint using arrow keys
Press enter to lock in a calibration point
"""
self._temp_calib = Calibration(cb_type)
with keyboard.Listener(
on_press = self._internal_on_press,
on_release = self._internal_on_release) as listener:
listener.join()
if not self._temp_calib.complete:
print "Quitting calibration without saving"
else:
self.calibration = self._temp_calib
if __name__=="__main__":
########################################
print "Testing joint angle subscriber"
......
from openrave_utils import *
from ur_kinematics import ur_kinematics
from openravepy import *
from time import sleep
import gcode2contour as gc
from math import pi
import numpy as np
from numpy import array
from numpy import matrix as mat
#from numpy.linalg import inv
def world2printTransform(point, Tr):
'''
world2printTransform
- This will convert world coordinates to robot coordinates.
- Additionally, it will set the angle of approach to be pointing down at the table
NOTE: This works when robot transforms are lateral, but not for rotations
Inputs:
- point: [0.3, 0, 1.1]
- Transformation matrix of robot relative to environment: (numpy matrix)
Output:
- Transformation matrix (numpy matrix) which can be fed directly to IK solver
'''
point.append(1)
transformed_point = np.concatenate(
# (mat([[1,0,0],[0,-1,0],[0,0,-1],[0,0,0]]), Tr * mat(point).T),
(mat([[0.0,-0.999,0.001],[-1.0,0.0,0.0],[0.0,-0.001,-0.999],[0,0,0]]), mat(point).T),
axis=1
)
transformed_point[3,3] = 1.0
return transformed_point
def interp_pose(pose1, pose2, t1, t2, ti):
"""
Linear interpolation of a pose between two times
Inputs:
- pose1 (list)
- pose2 (list)
- t1 (time at pose1)
- t2 (time at pose2)
- ti (time to interpolate for)
Output:
- pose_i (list)
"""
temp = (ti-t1)/(t2-t1)
return [p1 + (p2-p1)*temp for p1,p2 in zip(pose1, pose2)]
def show_contour(contour, env, linewidth=3):
"""
Display contour line in visualiser
Input: contour object (gcode2contour)
"""
return env.drawlinestrip(
points = array(contour.pos),
linewidth = linewidth,
colors=array((0,0,1))
)
def show_failed_contour(contour, env, linewidth=3):
"""
Display contour line in visualiser
Input: contour object (gcode2contour)
"""
return env.drawlinestrip(
points = array(contour.pos),
linewidth = linewidth,
colors=array((1,0,0))
)
def show_completed_contour(contour, env, linewidth=3):
"""
Display contour line in visualiser
Input: contour object (gcode2contour)
"""
return env.drawlinestrip(
points = array(contour.pos),
linewidth = linewidth,
colors=array((0,1,0))
)
#!/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 mm discrepency between true and desired => no solution
if (Tf - self.fk(th[0])).sum()>0.001:
th = []
print "IK not possible"
return th
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