Commit 43f913f8 authored by Lab Computer's avatar Lab Computer

Fix end effector to tool tip orientation

parent 4196ea11
calibration:
pos_ET: [0.14885085466107165, -0.052006706746552875, -0.03180325761311515]
pos_WR: [0.030510852445627096, -0.5832304614329086, -0.13143040024480324]
quat_ET: [-0.2705980500730985, -0.6532814824381882, -0.27059805007309845, 0.6532814824381883]
quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
quat_WR: [-0.0029816960949463804, 0.005867892094167384, -0.9999698348632657, 0.004123917349033938]
measurements:
errors:
......
calibration:
pos_ET: [0.1488171468456846, -0.04686485217586731, -0.035095146814831085]
pos_WR: [-0.02931384958017907, 0.58466302351586, -0.1316934641105142]
quat_ET: [-0.2705980500730985, -0.6532814824381882, -0.27059805007309845, 0.6532814824381883]
quat_WR: [0.008022261429018377, 0.008125316357774606, 0.010454855865521752, 0.9998801520904802]
quat_ET: [-0.2705980500730985, -0.6532814824381882, 0.27059805007309845, 0.6532814824381883]
quat_WR: [0.008022261429018376, 0.008125316357774606, 0.01045485586552175, 0.9998801520904802]
measurements:
errors:
- [5.184639014686743e-05, 0.0003069177484666683, 0.0010110215380649545]
- [0.00018167102090665965, -0.0007590811937544413, -0.0008193679078004334]
- [5.1846390146870897e-05, 0.00030691774846666785, 0.0010110215380649545]
- [0.00018167102090665965, -0.000759081193754443, -0.0008193679078004334]
- [0.000194391696983931, 0.0005494323899110087, 0.0003138569589627438]
- [-8.201190917067613e-05, 0.00028870454631119147, -0.0003138568851014656]
- [-0.0003458971987621018, -0.000385973491800173, -0.0001916537074498348]
......
......@@ -182,7 +182,7 @@ class Controller(object):
def getR_ET():
return Rotation.from_euler('xyz',[-45,-90,0],
return Rotation.from_euler('XYZ',[-45,-90,0],
degrees=True)
......
......@@ -23,7 +23,7 @@ from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest
from moveit_msgs.msg import MoveItErrorCodes
## Our libraries
from Controller import Controller
from Controller import Controller,getR_ET
class KeyboardController(Controller):
......@@ -201,16 +201,38 @@ class KeyboardController(Controller):
## Shift pose
# Rotate translation to end effector frame
trans_base = np.array([dx_m,dy_m,dz_m])
trans_ee = np.array([dx_m,dy_m,dz_m])
quat_curr = np.array([pose_curr.orientation.x,pose_curr.orientation.y,pose_curr.orientation.z,pose_curr.orientation.w])
R_curr = Rotation.from_quat(quat_curr)
trans_ee = R_curr.apply(trans_base)
trans_base = R_curr.apply(trans_ee)
## Translate as defined at base
self._translate_endpoint_base(trans_ee[0].item(),trans_ee[1].item(),trans_ee[2].item(),resolution_m)
# Translate as defined at base
self._translate_endpoint_base(trans_base[0].item(),trans_base[1].item(),trans_base[2].item(),resolution_m)
def _rotate_endpoint_base(self, dx_deg, dy_deg, dz_deg):
def _translate_endpoint_tool(self,dx_m,dy_m,dz_m,resolution_m=0.01):
"""
Translate the endpoint of the arm from where it currently is,
relative to the tool tip.
dx_m:
dy_m:
dz_m:
resolution_m: Largest end effector distance between two configurations
"""
## Get orientation of tool tip relative to end effector
R_ET = getR_ET()
## Shift pose
# Rotate translation to end effector frame
trans_tool = np.array([dx_m,dy_m,dz_m])
trans_ee = R_ET.apply(trans_tool)
# Translate as defined at end effector
self._translate_endpoint_ee(trans_ee[0].item(),trans_ee[1].item(),trans_ee[2].item(),resolution_m)
def _rotate_endpoint_base(self,dx_deg,dy_deg,dz_deg):
"""
Rotate the endpoint of the arm from where it currently is,
relative to the robot base.
......
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