def MoveEndPointToPosition(self,
pos=None,
rotation=None,
a=1.2,
v=0.25,
t=None):
if pos is not None:
pos = quat.qrotote_v(self.__base_q, pos, self.__base_t)
if rotation is not None:
if len(rotation) == 4:
rotation = quat.qmul(self.__base_q, rotation)
else:
rotation = quat.qmul(self.__base_q,
self.from_rad_axis_to_q(rotation))
super(HAPI, self).MoveEndPointToPosition(pos=pos,
rotation=rotation,
a=a,
v=v,
t=t)
def GetTargetEndPos(self, RAW=False):
p, q = super(HAPI, self).GetTargetEndPos()
p = quat.qrotote_v(quat.qconj(self.__base_q), p - self.__base_t)
q = quat.qmul(quat.qconj(self.__base_q), q)
if RAW:
q = self.from_q_to_rad_axis(q)
return np.asarray((p[0], p[1], p[2], q[0], q[1], q[2]),
dtype=np.float32)
else:
return p, q
def ForceMode(self,
taskframe=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0),
selection=(0, 0, 0, 0, 0, 0),
wrench=(0.1, 0.1, 0.1, 0.1, 0.1, 0.1),
limits=(0.2, 0.2, 0.2, 0.2, 0.2, 0.2),
duration=180):
p, q = taskframe[:3], self.from_rad_axis_to_q(taskframe[3:])
p = quat.qrotote_v(self.__base_q, p)
q = self.from_q_to_rad_axis(quat.qmul(self.__base_q, q))
super(HAPI,
self).ForceMode(taskframe=(p[0], p[1], p[2], q[0], q[1], q[2]),
selection=selection,
wrench=wrench,
limits=limits,
duration=duration)
def GetCurrentEndPos(self):
p, q = super(HAPI, self).GetCurrentEndPos()
p = quat.qrotote_v(quat.qconj(self.__base_q), p - self.__base_t)
q = quat.qmul(quat.qconj(self.__base_q), q)
return p, q
def FineTuningPosition(self,
interactive=False,
add_value=((0.01, 0.01, 0.01), (1.0, 0.0, 0.0,
0.0)),
factor=10.0):
"""
:param interactive: if True, call curses to capture keyboard input, else add add_value to current position
:param add_value: step size, for interactive mode, (dx, dy, dz, drx, dry, drz), else ((dx, dy, dz), (qw, qi, qj, qk)
:param factor: if in interactive mode, means key_hold boosting factor, the max speed will achieve after holding
a key for some time
:return: current position after fine tuning in format (x,y,z),(w,i,j,k)
"""
if interactive:
if len(add_value) == 6:
import curses
stdscr = curses.initscr()
curses.noecho()
curses.cbreak()
stdscr.keypad(1)
stdscr.nodelay(1)
stdscr.addstr("Start Interactive Fine Tune Mode\n\r")
stdscr.addstr(
"Press 'a','d' for moving along x axis, 'A', 'D' for rotate\n\r"
)
stdscr.addstr(
"Press 's','w' for moving along y axis, 'S', 'W' for rotate\n\r"
)
stdscr.addstr(
"Press 'q','e' for moving along z axis, 'Q', 'E' for rotate\n\r"
)
stdscr.addstr("Press 'esc' to exit >>>\n\r")
step = list(add_value)
while 1:
char = stdscr.getch()
curses.flushinp()
p, q = self.GetCurrentEndPos()
stdscr.clrtoeol()
rot = self.from_q_to_rad_axis(q)
stdscr.addstr("\r(x=%f, y=%f, z=%f, rx=%f, ry=%f, rz=%f)" %
(p[0], p[1], p[2], rot[0], rot[1], rot[2]))
if char == curses.KEY_LEFT or char == ord('a'): # -X
p[0] -= step[0]
if step[0] < add_value[0] * factor:
step[0] += add_value[0] * factor / 10.0
elif char == curses.KEY_RIGHT or char == ord('d'): # +X
p[0] += step[0]
if step[0] < add_value[0] * factor:
step[0] += add_value[0] * factor / 10.0
elif char == curses.KEY_UP or char == ord('w'): # +Y
p[1] += step[1]
if step[1] < add_value[1] * factor:
step[1] += add_value[1] * factor / 10.0
elif char == curses.KEY_DOWN or char == ord('s'): # -Y
p[1] -= step[1]
if step[1] < add_value[1] * factor:
step[1] += add_value[1] * factor / 10.0
elif char == curses.KEY_PPAGE or char == ord('e'): # +Z
p[2] += step[2]
if step[2] < add_value[2] * factor:
step[2] += add_value[2] * factor / 10.0
elif char == curses.KEY_NPAGE or char == ord('q'): # -Z
p[2] -= step[2]
if step[2] < add_value[2] * factor:
step[2] += add_value[2] * factor / 10.0
elif char == ord('A'): # -rX
q = quat.qmul(
q, self.from_rad_axis_to_q((-step[3], 0, 0)))
if step[3] < add_value[3] * factor:
step[3] += add_value[3] * factor / 10.0
elif char == ord('D'): # +rX
q = quat.qmul(q,
self.from_rad_axis_to_q((step[3], 0, 0)))
if step[3] < add_value[3] * factor:
step[3] += add_value[3] * factor / 10.0
elif char == ord('W'): # +rY
q = quat.qmul(q,
self.from_rad_axis_to_q((0, step[4], 0)))
if step[4] < add_value[4] * factor:
step[4] += add_value[4] * factor / 10.0
elif char == ord('S'): # -rY
q = quat.qmul(
q, self.from_rad_axis_to_q((0, -step[4], 0)))
if step[4] < add_value[4] * factor:
step[4] += add_value[4] * factor / 10.0
elif char == ord('E'): # +rZ
q = quat.qmul(q,
self.from_rad_axis_to_q((0, 0, step[5])))
if step[5] < add_value[5] * factor:
step[5] += add_value[5] * factor / 10.0
elif char == ord('Q'): # -rZ
q = quat.qmul(
q, self.from_rad_axis_to_q((0, 0, -step[5])))
if step[5] < add_value[5] * factor:
step[5] += add_value[5] * factor / 10.0
elif char == 27:
break
else:
step = list(add_value)
continue
self.MoveEndPointToPosition(pos=p,
rotation=q,
v=0.1,
a=0.25)
time.sleep(0.1)
curses.endwin()
print("Exit Interactive Fine Tune Mode")
else:
print(
"Error add_value format, need (dx, dy, dz, drx, dry, drz) but get",
add_value)
else:
if len(add_value) == 2 and len(add_value[0]) == 3 and len(
add_value[1]) == 4:
p, q = self.GetCurrentEndPos()
p_next = p + np.asarray(add_value[0], dtype=np.float32)
q_next = quat.qmul(add_value[1], q)
self.MoveEndPointToPosition(pos=p_next, rotation=q_next)
while not self.isLastMovementEnd():
time.sleep(0.5)
else:
print(
"Error add_value format, need ((dx, dy, dz), (qw, qi, qj, qk)) but get",
add_value)
return self.GetCurrentEndPos()