def ts_project(self, js_traj, phi_start = 0.0, phi_end = None):
'''
projects the given jointspace trajectory into the taskspace
The phase starts from phi_start and added by delta_phi in each step.
if at any time the joint values are not feasible, the process is stopped.
'''
if phi_end == None:
phi_end = js_traj.phi_end
pt = trajlib.Trajectory_Polynomial(dimension = 3*len(self.task_point) + 9*len(self.task_frame))
if phi_end > js_traj.phi_end:
phi_end = js_traj.phi_end
phi = phi_start
stay = True
while stay:
if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step):
phi = phi_end
stay = False
js_traj.set_phi(phi)
if self.set_config(js_traj.current_position):
pt.add_point(phi - phi_start, self.pose())
phi = phi + self.ik_settings.time_step
return pt
def read_trajectory():
# Read Matlab Workspace File:
data_file = "pr2_trajectory_data_v2.mat"
workspace = scipy.io.loadmat(data_file)
p = workspace['p']
q = workspace['q']
N = len(p)
assert N == len(q)
## Create a position and orientation trajectory from given data:
pt = traj.Trajectory_Polynomial(capacity=5)
ot = traj.Orientation_Trajectory_Polynomial()
t = 0.0
dt = 0.005
for i in range(N):
pos = p[i, :]
ori = q[i, :]
# pos = np.array(pos[0], [pos[1], pos[2]])
pt.add_point(t, pos=pos)
ot.add_point(t,
ori=geo.Orientation_3D(ori, representation='quaternion'))
t = t + dt
ot.consistent_velocities()
pt.consistent_velocities()
return (pt, ot)
def js_create(self, phi_end = None, traj_capacity = 2, traj_type = 'regular'):
'''
'''
self.damping_factor = self.ik_settings.initial_damping_factor
if self.ik_settings.generate_log:
self.run_log.clear()
keep_q = np.copy(self.q)
H = self.transfer_matrices()
if phi_end == None:
phi_end = self.ik_settings.number_of_steps*self.ik_settings.time_step
if traj_type == 'regular':
jt = trajlib.Trajectory(dimension = self.config_settings.DOF, capacity = traj_capacity)
elif traj_type == 'polynomial':
jt = trajlib.Trajectory_Polynomial(dimension = self.config_settings.DOF, capacity = traj_capacity)
else:
assert False, "\n Unknown Trajectory Type \n"
jt.plot_settings.axis_label = self.config_settings.joint_label
if self.ik_settings.respect_limits_in_trajectories:
jt.vel_max = self.ik_settings.max_js
jt.acc_max = self.ik_settings.max_ja
jt.pos_max = self.config_settings.qh
jt.pos_min = self.config_settings.ql
phi = 0.0
jt.add_position(0.0, pos = np.copy(self.q))
stay = not self.in_target()
while stay:
stay = not self.in_target()
phi = phi + self.ik_settings.time_step
if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step):
phi = phi_end
stay = False
err_reduced = self.moveto_target()
if err_reduced or (not self.ik_settings.respect_error_reduction):
jt.add_position(phi = phi, pos = np.copy(self.q))
if traj_type == 'polynomial':
jt.interpolate()
self.set_config(keep_q)
if self.ik_settings.generate_log:
self.run_log.time_elapsed = time.time() - self.run_log.time_start
return jt
def draw_from_canvas(self,
canvas_shapes,
width_pixels=640,
height_pixels=480,
plot=False,
silent=True):
self.sync_object()
ori_l = self.larm.wrist_orientation()
ori_r = self.rarm.wrist_orientation()
wr = -ori_r[:, 0]
hr = ori_r[:, 1]
nr = ori_r[:, 2]
wl = -ori_l[:, 0]
hl = ori_l[:, 1]
nl = ori_l[:, 2]
self.control_mode = "Fixed-Base"
self.larm_reference = False
self.rarm_startpoint = self.rarm_startpoint + nr * self.depth
self.larm_startpoint = self.larm_startpoint + nl * self.depth
pr_list = []
pl_list = []
for cs in canvas_shapes:
if cs.arm == "R":
phi = 1.0
(x0, y0) = cs.pts[0]
pos0 = self.rarm_startpoint + self.box_width_rarm * x0 * wr / width_pixels + self.box_height_rarm * (
height_pixels - y0) * hr / height_pixels
pr = traj.Trajectory_Polynomial(dimension=3)
pr.add_point(0.0, np.copy(pos0), np.zeros(3))
for (x, y) in cs.pts[1:]:
pos = self.rarm_startpoint + self.box_width_rarm * x * wr / width_pixels + self.box_height_rarm * (
height_pixels - y) * hr / height_pixels
pr.add_point(phi, pos)
# pr.new_segment()
phi += 1.0
pr.consistent_velocities()
pr_list.append(pr)
if plot:
pr.plot3d()
elif cs.arm == "L":
phi = 1.0
(x0, y0) = cs.pts[0]
pos0 = self.larm_startpoint + self.box_width_larm * x0 * wl / width_pixels + self.box_height_larm * (
height_pixels - y0) * hl / height_pixels
pl = traj.Trajectory_Polynomial(dimension=3)
pl.add_point(0.0, np.copy(pos0), np.zeros(3))
for (x, y) in cs.pts[1:]:
pos = self.larm_startpoint + self.box_width_larm * x * wl / width_pixels + self.box_height_larm * (
height_pixels - y) * hl / height_pixels
pl.add_point(phi, pos)
# pl.new_segment()
phi += 1.0
pl.consistent_velocities()
pl_list.append(pl)
if plot:
pr.plot3d()
else:
assert False
ir = 0
il = 0
nrt = len(pr_list)
nlt = len(pl_list)
step_r = 0
step_l = 0
t0 = time.time()
t = 0.0
# while ((ir < nrt) or (il < nlt)) and (not pint.rarm_failed) and (not pint.larm_failed) and (t < self.timeout):
while (
(ir < nrt) or
(il < nlt)) and (not pint.rarm_failed) and (not pint.larm_failed):
# initial positioning:
if (ir < nrt) and (step_r == 0):
t0 = time.time()
if not silent:
print "Trajectory: ", ir, " Step 0 for Right Arm"
pr = pr_list[ir]
pos = np.copy(pr.segment[0].point[0].pos)
pos = pos - self.depth * nr
self.rarm.set_target(pos, ori_r)
assert self.rarm.goto_target(optimize=True)
pint.take_rarm_to(self.rarm.config.q, time_to_reach=2.0)
step_r += 1
if (il < nlt) and (step_l == 0):
t0 = time.time()
if not silent:
print "Trajectory: ", il, " Step 0 for Left Arm"
pl = pl_list[il]
pos = np.copy(pl.segment[0].point[0].pos)
pos = pos - self.depth * nl
self.larm.set_target(pos, ori_l)
assert self.larm.goto_target(optimize=True)
pint.take_larm_to(self.larm.config.q, time_to_reach=2.0)
step_l += 1
# head forward:
if (pint.rarm_reached) and (step_r == 1):
t0 = time.time()
if not silent:
print "Trajectory: ", ir, " Step 1 for Right Arm"
pos = np.copy(pr.segment[0].point[0].pos)
self.rarm.set_target(pos, ori_r)
assert self.rarm.goto_target(optimize=True)
pint.take_rarm_to(self.rarm.config.q, time_to_reach=1.0)
step_r += 1
if (pint.larm_reached) and (step_l == 1):
t0 = time.time()
if not silent:
print "Trajectory: ", il, " Step 1 for Left Arm"
pos = np.copy(pl.segment[0].point[0].pos)
self.larm.set_target(pos, ori_l)
assert self.larm.goto_target(optimize=True)
pint.take_larm_to(self.larm.config.q, time_to_reach=1.0)
step_l += 1
# run trajectory:
if (pint.rarm_reached) and (step_r == 2):
t0 = time.time()
if not silent:
print "Trajectory: ", ir, " Step 2 for Right Arm"
self.larm_reference = False
# self.arm_trajectory(pr, relative = False, delta_phi = 0.5, wait = False)
self.draw_shape(pr)
step_r += 1
if (pint.larm_reached) and (step_l == 2):
t0 = time.time()
if not silent:
print "Trajectory: ", il, " Step 2 for Left Arm"
self.larm_reference = True
# self.arm_trajectory(pl, relative = False, delta_phi = 0.5, wait = False)
nseg = len(pl.segment)
# print "len(pl.segment)", nseg
# print "num.points.last.seg:", len(pl.segment[nseg-1].point)
#print pl.segment[nseg-1].point[0]
#print pl.segment[nseg-1].point[1]
#print pl.segment[nseg-1].points_dist()
self.draw_shape(pl)
step_l += 1
# head backward:
if (pint.rarm_reached) and (step_r == 3):
t0 = time.time()
if not silent:
print "Trajectory: ", ir, " Step 3 for Right Arm"
self.sync_object()
pos = self.rarm.wrist_position() - nr * self.depth
self.rarm.set_target(pos, ori_r)
assert self.rarm.goto_target(optimize=True)
pint.take_rarm_to(self.rarm.config.q, time_to_reach=1.0)
step_r += 1
if (pint.larm_reached) and (step_l == 3):
t0 = time.time()
if not silent:
print "Trajectory: ", il, " Step 3 for Left Arm"
self.sync_object()
pos = self.larm.wrist_position() - nl * self.depth
self.larm.set_target(pos, ori_l)
assert self.larm.goto_target(optimize=True)
pint.take_larm_to(self.larm.config.q, time_to_reach=1.0)
'''
self.larm_reference = True
self.arm_back(dx = self.depth, relative = True)
'''
step_l += 1
if (pint.rarm_reached) and (step_r == 4):
t0 = time.time()
if not silent:
print "Trajectory: ", ir, " Step 4 for Right Arm"
ir += 1
step_r = 0
if (pint.larm_reached) and (step_l == 4):
t0 = time.time()
if not silent:
print "Trajectory: ", il, " Step 4 for Left Arm"
il += 1
step_l = 0
time.sleep(0.100)
t = time.time() - t0
self.sync_object()
self.rarm_startpoint = self.rarm_startpoint - nr * self.depth
self.larm_startpoint = self.larm_startpoint - nl * self.depth
if t > self.timeout:
print "Action not completed! Time Out!"
# Return back to start point:
if nrt > 0:
self.rarm.set_target(self.rarm_startpoint, ori_r)
self.rarm_target(wait=True)
if nlt > 0:
self.larm.set_target(self.larm_startpoint, ori_l)
self.larm_target(wait=True)
def js_project(self, pos_traj, ori_traj = None, phi_start = 0.0, phi_end = None, relative = True, traj_capacity = 2, traj_type = 'regular'):
'''
projects the given taskspace pose trajectory into the jointspace using the numeric inverse kinematics.
The phase starts from phi_start and added by self.ik_settings.time_step in each step.
at any time, if a solution is not found, the target is ignored and no configuration is added to the trajectory
'''
self.damping_factor = self.ik_settings.initial_damping_factor
keep_q = np.copy(self.q)
H = self.transfer_matrices()
if ori_traj == None:
ori_traj = trajlib.Orientation_Path()
ori_traj.add_point(0.0, self.task_frame[0].orientation(H))
ori_traj.add_point(pos_traj.phi_end, self.task_frame[0].orientation(H))
if phi_end == None:
phi_end = min(pos_traj.phi_end, ori_traj.phi_end)
if (phi_end > pos_traj.phi_end) or (phi_end > ori_traj.phi_end):
phi_end = min(pos_traj.phi_end, ori_traj.phi_end)
if traj_type == 'regular':
jt = trajlib.Trajectory(dimension = self.config_settings.DOF, capacity = traj_capacity)
elif traj_type == 'polynomial':
jt = trajlib.Trajectory_Polynomial(dimension = self.config_settings.DOF, capacity = traj_capacity)
else:
assert False, "\n Unknown Trajectory Type \n"
if self.ik_settings.respect_limits_in_trajectories:
jt.vel_max = self.ik_settings.max_js
jt.acc_max = self.ik_settings.max_ja
jt.pos_max = self.config_settings.qh
jt.pos_min = self.config_settings.ql
phi = phi_start
pos_traj.set_phi(phi)
ori_traj.set_phi(phi)
if relative:
p0 = self.task_point[0].position(H) - pos_traj.current_position
self.set_target([self.task_point[0].position(H)], [self.task_frame[0].orientation(H)])
else:
p0 = np.zeros(3)
self.set_target([pos_traj.current_position], [ori_traj.current_orientation])
self.goto_target()
jt.add_position(0.0, pos = np.copy(self.q))
if self.ik_settings.generate_log:
self.run_log.clear()
self.run_log.step.append(self.report_status())
stay = True
while stay:
phi = phi + self.ik_settings.time_step
if (phi > phi_end) or genmath.equal(phi, phi_end, epsilon = 0.1*self.ik_settings.time_step):
phi = phi_end
stay = False
pos_traj.set_phi(phi)
ori_traj.set_phi(phi)
p = p0 + pos_traj.current_position
self.set_target([p], [ori_traj.current_orientation])
err_reduced = self.moveto_target()
if err_reduced or (not self.ik_settings.respect_error_reduction):
jt.add_position(phi = phi - phi_start, pos = np.copy(self.q))
if traj_type == 'polynomial':
jt.interpolate()
self.set_config(keep_q)
if self.ik_settings.generate_log:
self.run_log.time_elapsed = time.time() - self.run_log.time_start
return jt