Commit e47cb668 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

calibration working but ur kinematics is not working - need to find a replacement

parent 0e9e6c12
from copy import copy, deepcopy
from pynput import keyboard
from enum import Enum
from utils import ur_kinematics
from math import pi
import time
import warnings
......@@ -60,6 +62,7 @@ class controller:
self.joint_state = None
self.calibration = None
self._temp_calib = None
self.kin = ur_kinematics("UR5e")
time.sleep(1)
......@@ -71,6 +74,35 @@ class controller:
self.traj.publish(traj)
def move_to(self, joint_angles, max_speed = pi/10):
# TODO TODO linear interpolation for larger angles - if needed?
"""
move the robot to particular joint angle
gets current joints and linear interps to desired joints
and executes
max speed is in rad/s, deafault 10s to do a 180
"""
now = self.get_joints()
max_dTheta = max([abs(now[a]-joint_angles[a]) for a in range(len(now))])
total_time = max_dTheta/max_speed
t = self.empty_traj()
p1 = JointTrajectoryPoint()
p1.positions = copy(now)
p1.time_from_start = rospy.Time(0)
p2 = JointTrajectoryPoint()
p2.positions = copy(tuple(joint_angles))
p2.time_from_start = rospy.Time(total_time)
t.points.append(p1)
t.points.append(p2)
print t
self.exec_traj(t)
def get_joints(self):
"""
Get current state of UR5
......@@ -94,6 +126,34 @@ class controller:
return t
def _internal_move_endpoint(self, dx, dy, dz):
"""
move the endpoint of the arm from where it currenlty is
"""
# step 1 get current point angles
now = self.get_joints()
# step 2 forward kinemtics giving transformation matrix
T = self.kin.fk(list(now))
# step 3 Shift transformation matrix in desired direction
T[0,3] += dx
T[1,3] += dy
T[2,3] += dz
# step 4 do inverse kinematics
js = self.kin.ik(T)
# step 5 choose closest joint angles to current angle
j, max_dTheta = self.kin.get_closest(js, list(now))
# step 6 move to new angle
if max_dTheta < pi/10:
self.move_to(j)
else:
print "ERROR"
def _internal_on_press(self, key):
"""
CALIBRATION component:
......@@ -102,23 +162,23 @@ class controller:
"""
if key == keyboard.Key.up:
pass
self._internal_move_endpoint( 0.005, 0, 0)
elif key == keyboard.Key.down:
pass
self._internal_move_endpoint(-0.005, 0, 0)
elif key == keyboard.Key.left:
pass
self._internal_move_endpoint(0, 0.005, 0)
elif key == keyboard.Key.right:
pass
self._internal_move_endpoint(0, -0.005, 0)
elif key == keyboard.Key.page_up:
pass
self._internal_move_endpoint(0, 0, 0.005)
elif key == keyboard.Key.page_down:
pass
self._internal_move_endpoint(0, 0, -0.005)
elif key == keyboard.Key.enter:
# save current pose into calibaration
self._temp_calib.push_parameter(self.get_joints())
def _internal_on_release(key):
def _internal_on_release(self, key):
"""
Stop calibration if escape key pressed
"""
......
......@@ -70,6 +70,7 @@ class ur_kinematics:
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):
......@@ -165,10 +166,40 @@ class ur_kinematics:
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:
#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