Commit 5f991edc authored by Jayant Khatkar's avatar Jayant Khatkar

fast calibration working but unsafe (#18)

parent 32a41a49
......@@ -45,12 +45,11 @@ class Calibrator(object):
)
# form prompts to the user to satisfy
prompts = ['Move robot tip to {}'.format(np.array2string(pos_W[iPos,]))
for iPos in range(pos_W.shape[0])]
prompts = [pos_W[iPos,] for iPos in range(pos_W.shape[0])]
# gather configurations
# [ ( [joint angles] , ROS pose message ) , ... ]
configurations = self.controller.prompt_configurations(prompts)
configurations = self.controller.prompt_configurations(prompts, prior_calibs)
pos_RE = [[c[1].pose.position.x, c[1].pose.position.y, c[1].pose.position.z]
for c in configurations]
......
......@@ -54,6 +54,7 @@ class Controller(object):
- wrist_2_joint
- wrist_3_joint
"""
def __init__(self,
close_deg=1.0,
timeScale=1.0,
......@@ -257,7 +258,7 @@ class Controller(object):
return True
def move_to(self,conf_next,near_deg = 1.0):
def move_to(self, conf_next, near_deg = 1.0):
"""
Moves the arm to the specified configuration by linearly
interpolating the joint angles.
......@@ -278,14 +279,14 @@ class Controller(object):
dist_rad = np.linalg.norm(dConf)
dist_deg = np.rad2deg(dist_rad)
if dist_deg > near_deg:
rospy.logwarn('Attempted to move a joint distance of {} > {} deg'.format(dist_conf_deg,jointLim_deg))
rospy.logwarn('Attempted to move a joint distance of {} > {} deg'.format(dConf, near_deg))
return False
self.group.go(conf_next)
return True
def is_close(self,trajPoint):
def is_close(self, trajPoint):
"""
Check if the arm is currently close to the configuration specified.
Ignores timestamp.
......
......@@ -17,7 +17,7 @@ import numpy as np
# ROS Libraries
import rospy
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
from moveit_msgs.srv import GetPositionFK,GetPositionFKRequest
from moveit_msgs.srv import GetPositionIK,GetPositionIKRequest
from moveit_msgs.msg import MoveItErrorCodes
......@@ -26,6 +26,20 @@ from moveit_msgs.msg import MoveItErrorCodes
from Controller import Controller,getR_ET
# used for fast-calibration
def getTransformMat(quat, pos):
T = np.identity(4)
T[:3,:3] = Rotation.from_quat(quat).as_dcm()
T[:3,3] = pos
return np.linalg.inv(T)
# also used for fast-calibration-ik
_vert_or = np.array([[-6.74517010e-04, 8.33315658e-04, -9.99999404e-01, -2.90597873e-05],
[ 8.33732192e-04, -9.99999285e-01, -8.33877944e-04, -4.81068309e-05],
[-9.99999404e-01, -8.34294187e-04, 6.73821778e-04, 2.74792069e-06],
[ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
class KeyboardController(Controller):
"""
Controller to control robot arms by keyboard.
......@@ -163,9 +177,9 @@ class KeyboardController(Controller):
Translate the endpoint of the arm from where it currently is,
relative to the robot base.
dx_m:
dy_m:
dz_m:
dx_m:
dy_m:
dz_m:
resolution_m: Largest end effector distance between two configurations
"""
## Get current pose
......@@ -200,9 +214,9 @@ class KeyboardController(Controller):
Translate the endpoint of the arm from where it currently is,
relative to the end effector.
dx_m:
dy_m:
dz_m:
dx_m:
dy_m:
dz_m:
resolution_m: Largest end effector distance between two configurations
"""
## Get current pose
......@@ -224,9 +238,9 @@ class KeyboardController(Controller):
Translate the endpoint of the arm from where it currently is,
relative to the tool tip.
dx_m:
dy_m:
dz_m:
dx_m:
dy_m:
dz_m:
resolution_m: Largest end effector distance between two configurations
"""
## Get orientation of tool tip relative to end effector
......@@ -525,7 +539,7 @@ class KeyboardController(Controller):
pass
def prompt_configurations(self,prompt=None):
def prompt_configurations(self, prompt=None, prior_calibs=None):
"""
Prompts user to control the robot to a series of configurations.
Relative to robot base, the robot can be translated in the:
......@@ -558,18 +572,45 @@ class KeyboardController(Controller):
'step_deg': 11.25,
'success': True}
if isinstance(prompt,list):
# if previous calib data, create transformation matrix to calc approx IKs
if prior_calibs is not None and len(prior_calibs)>0:
world2armT = getTransformMat(prior_calibs['quat_WR'], prior_calibs['pos_WR'])
tip2armT = getTransformMat([0,0,0,1], prior_calibs['pos_ET'])
priors = True
else:
priors = False
if isinstance(prompt, list):
for p in prompt:
print(p)
print('Move robot tip to {}'.format(str(p)))
if priors:
# calculate IK for next point and move there
mat = np.identity(4)
mat[:3,3] = p
mat = world2armT.dot(mat).dot(_vert_or).dot(tip2armT)
q = Rotation.from_dcm(mat[:3,:3]).as_quat()
p_r = mat[0:3,3]
gpose = Pose()
gpose.position.x, gpose.position.y, gpose.position.z = p_r
gpose.orientation.x, gpose.orientation.y, \
gpose.orientation.z, gpose.orientation.w = q
self.group.set_pose_target(gpose)
self.group.go(wait=True)
with keyboard.Listener(
on_release = lambda key:self._move_callback(key,configurations,params),
suppress=True) as listener:
listener.join()
if DEBUG:
print('Keyboard listener thread joined')
if not params['success']:
sys.exit(0)
elif isinstance(prompt,int):
elif isinstance(prompt, int):
for iConfig in range(prompt):
print('Enter configuration {}'.format(iConfig))
with keyboard.Listener(
......@@ -580,6 +621,7 @@ class KeyboardController(Controller):
print('Keyboard listener thread joined')
if not params['success']:
sys.exit(0)
else:
while params['success']:
params['success'] = False
......@@ -594,14 +636,14 @@ class KeyboardController(Controller):
if __name__=="__main__":
#con = KeyboardController(disableExtruder=True)
con = KeyboardController(disableExtruder=True)
#con = KeyboardController()
#con1 = KeyboardController(robot=1)
con2 = KeyboardController(robot=2, disableExtruder=True)
#con2 = KeyboardController(robot=2, disableExtruder=True)
print('Collecting configrations...')
#configuations = con.prompt_configurations()
configuations = con.prompt_configurations()
#configuations = con1.prompt_configurations()
configuations = con2.prompt_configurations()
#configuations = con2.prompt_configurations()
print('Your configurations:')
print(configuations)
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