Commit e47cb668 by Jayant Khatkar

### 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!