def update_eq_point(self, arm, motion_vec, step_size, rot_mat):
x,y,a,dx,dy,da = 0.,0.,0.,0.,0.,0.
st = self.segway_trajectory
if len(st.x_list)>0:
x,y,a = st.x_list[-1],st.y_list[-1],st.a_list[-1]
if len(st.x_list)>1:
dx = x-st.x_list[-2]
dy = y-st.y_list[-2]
da = tr.angle_within_mod180(x-st.a_list[-2])
self.eq_pt_cartesian = smc.tlTts(self.eq_pt_cartesian_ts,x,y,a)
if len(self.zenither_list) > 2:
h = self.zenither_list[-1] - self.zenither_list[-2]
self.eq_pt_cartesian[2,0] -= h
next_pt = self.eq_pt_cartesian + step_size * motion_vec
if self.q_guess != None:
if arm == 'right_arm':
self.q_guess[1] = ut.bound(self.q_guess[1],math.radians(20),math.radians(5))
q_eq = self.firenze.IK(arm,next_pt,rot_mat,self.q_guess)
self.eq_pt_cartesian = next_pt
self.eq_pt_cartesian_ts = smc.tsTtl(self.eq_pt_cartesian,x,y,a)
self.q_guess = q_eq
return q_eq
def account_segway_motion(cart_traj,st):
ct = CartesianTajectory()
for i in range(len(cart_traj.p_list)):
x,y,a = st.x_list[i], st.y_list[i], st.a_list[i]
p_tl = np.matrix(cart_traj.p_list[i]).T
p_ts = smc.tsTtl(p_tl, x, y, a)
p = p_ts
ct.p_list.append(p.A1.tolist())
# this is incorrect. I also need to use the velocity of the
# segway. Unclear whether this is useful right now, so not
# implementing it. (Advait. Jan 6, 2010.)
if cart_traj.v_list != []:
v_tl = np.matrix(cart_traj.v_list[i]).T
v_ts = smc.tsRtl(v_tl, a)
ct.v_list.append(v_ts.A1.tolist())
ct.time_list = copy.copy(cart_traj.time_list)
return ct
def account_segway_motion(cart_traj,st):
ct = CartesianTajectory()
for i in range(len(cart_traj.p_list)):
x,y,a = st.x_list[i], st.y_list[i], st.a_list[i]
p_tl = np.matrix(cart_traj.p_list[i]).T
p_ts = smc.tsTtl(p_tl, x, y, a)
p = p_ts
ct.p_list.append(p.A1.tolist())
# this is incorrect. I also need to use the velocity of the
# segway. Unclear whether this is useful right now, so not
# implementing it. (Advait. Jan 6, 2010.)
if cart_traj.v_list != []:
v_tl = np.matrix(cart_traj.v_list[i]).T
v_ts = smc.tsRtl(v_tl, a)
ct.v_list.append(v_ts.A1.tolist())
ct.time_list = copy.copy(cart_traj.time_list)
return ct
def equi_pt_generator_control_radial_force(self, arm, rot_mat,
h_force_possible, v_force_possible, cep_vel):
self.log_state(arm)
step_size = 0.1 * cep_vel # 0.1 is the time interval between calls to the equi_generator function (see pull)
q_eq = self.update_eq_point(arm, self.eq_motion_vec, step_size,
rot_mat)
stop = self.common_stopping_conditions()
wrist_force = self.firenze.get_wrist_force(arm, base_frame=True)
mag = np.linalg.norm(wrist_force)
curr_pos_tl = self.firenze.FK(arm,self.pull_trajectory.q_list[-1])
st = self.segway_trajectory
x,y,a = st.x_list[-1],st.y_list[-1],st.a_list[-1]
curr_pos_ts = smc.tsTtl(curr_pos_tl,x,y,a)
curr_pos = curr_pos_ts
if len(self.zenither_list) > 1:
h = self.zenither_list[-1] - self.zenither_list[0]
curr_pos[2,0] += h
if len(self.pull_trajectory.q_list)>1:
start_pos = np.matrix(self.cartesian_pts_list[0]).T
else:
start_pos = curr_pos
#mechanism kinematics.
self.mech_traj_pub.publish(Point32(curr_pos[0,0],
curr_pos[1,0], curr_pos[2,0]))
if self.use_jacobian:
self.mech_kinematics_lock.acquire()
self.cartesian_pts_list.append(curr_pos.A1.tolist())
tangential_vec_ts = self.tangential_vec_ts
force_vec_ts = self.constraint_vec_1_ts + self.constraint_vec_2_ts
# this is specifically for the right arm, and lots of
# other assumptions about the hook etc. (Advait, Feb 28, 2010)
if h_force_possible:
force_vec_ts[2,0] = 0.
if v_force_possible:
force_vec_ts[1,0] = 0.
force_vec_ts = force_vec_ts / np.linalg.norm(force_vec_ts)
if force_vec_ts[2,0] < 0.: # only allowing upward force.
force_vec_ts = -force_vec_ts
if force_vec_ts[1,0] < 0.: # only allowing force to the left
force_vec_ts = -force_vec_ts
self.mech_kinematics_lock.release()
force_vec = smc.tlRts(force_vec_ts, a)
tangential_vec = smc.tlRts(tangential_vec_ts, a)
print 'tangential vec right at the transformation:', tangential_vec.A1
print 'tangential vec ts right at the transformation:', tangential_vec_ts.A1
print 'a:', a
if self.use_rotation_center:
self.fit_circle_lock.acquire()
self.cartesian_pts_list.append(curr_pos.A1.tolist())
rad = self.rad
cx_start, cy_start = self.cx_start, self.cy_start
cz_start = self.cz_start
self.fit_circle_lock.release()
c_ts = np.matrix([cx_start,cy_start,0.]).T
c_tl = smc.tlTts(c_ts,x,y,a)
cx,cy = c_tl[0,0],c_tl[1,0]
cz = cz_start
z_start = start_pos[2,0]
parallel_to_floor = abs(z_start - cz) < 0.1
print 'abs(z_start - cz)', abs(z_start - cz)
if parallel_to_floor:
print 'Mechanism is in the XY plane.'
# trajectory is parallel to the ground.
# find tangential direction.
radial_vec_tl = curr_pos_tl-np.matrix([cx,cy,cz]).T
radial_vec = radial_vec_tl
radial_vec = radial_vec/np.linalg.norm(radial_vec)
if cy_start<start_pos[1,0]:
# if cy<0.:
tan_x,tan_y = -radial_vec[1,0],radial_vec[0,0]
else:
tan_x,tan_y = radial_vec[1,0],-radial_vec[0,0]
if tan_x > 0. and (start_pos[0,0]-curr_pos[0,0]) < 0.09:
tan_x = -tan_x
tan_y = -tan_y
if cy_start>start_pos[1,0]:
radial_vec = -radial_vec # axis to the left, want force in
# anti-radial direction.
rv = radial_vec
force_vec = np.matrix([rv[0,0], rv[1,0], 0.]).T
tangential_vec = np.matrix([tan_x, tan_y, 0.]).T
else:
print 'Mechanism is in the XZ plane.'
# here the mechanism does not move in a plane parallel
# to the ground.
radial_vec_tl = curr_pos_tl-np.matrix([cx,cy,cz]).T
radial_vec = radial_vec_tl
radial_vec = radial_vec/np.linalg.norm(radial_vec)
tan_x, tan_z = -radial_vec[2,0], radial_vec[0,0]
if tan_x > 0. and (start_pos[0,0]-curr_pos[0,0]) < 0.09:
tan_x = -tan_x
tan_z = -tan_z
rv = radial_vec
force_vec = np.matrix([rv[0,0], 0., rv[2,0]]).T
tangential_vec = np.matrix([tan_x, 0., tan_z]).T
if abs(curr_pos[2,0] - z_start) > 0.03 and parallel_to_floor:
force_vec += np.matrix([0.,0.,1]).T
parallel_to_floor = False
tangential_vec_ts = smc.tsRtl(tangential_vec, a)
radial_vec_ts = smc.tsRtl(radial_vec, a)
force_vec_ts = smc.tsRtl(force_vec, a)
f_vec = -1*np.array([wrist_force[0,0], wrist_force[1,0],
wrist_force[2,0]])
f_rad_mag = np.dot(f_vec, force_vec.A1)
err = f_rad_mag-5.
if err>0.:
kp = -0.1
else:
kp = -0.2
radial_motion_mag = kp * err # radial_motion_mag in cm (depends on eq_motion step size)
if self.use_rotation_center:
if h_force_possible == False and parallel_to_floor:
radial_motion_mag = 0.
if v_force_possible == False and parallel_to_floor == False:
radial_motion_mag = 0.
radial_motion_vec = force_vec * radial_motion_mag
self.eq_motion_vec = copy.copy(tangential_vec)
self.eq_motion_vec += radial_motion_vec
if self.move_segway_flag:
self.segway_motion_local(curr_pos_tl, a)
self.prev_force_mag = mag
if self.init_tangent_vector == None:
self.init_tangent_vector = copy.copy(tangential_vec_ts)
c = np.dot(tangential_vec_ts.A1, self.init_tangent_vector.A1)
ang = np.arccos(c)
dist_moved = np.dot((curr_pos - start_pos).A1, self.tangential_vec_ts.A1)
ftan = abs(np.dot(wrist_force.A1, tangential_vec.A1))
# print 'wrist force:', wrist_force.A1
# print 'tangential_vec:', tangential_vec.A1
# print 'tangential_vec_ts:', tangential_vec_ts.A1
# print 'ftan:', ftan
# print 'force magnitude:', mag
if abs(dist_moved) > 0.09 and self.hooked_location_moved == False:
# change the force threshold once the hook has started pulling.
self.hooked_location_moved = True
self.eq_force_threshold = ut.bound(mag+30.,20.,80.)
self.ftan_threshold = self.ftan_threshold + max(10., 1.5*ftan)
if self.hooked_location_moved:
if abs(tangential_vec_ts[2,0]) < 0.2 and ftan > self.ftan_threshold:
# print 'ftan threshold exceed.'
stop = 'ftan threshold exceed: %f'%ftan
else:
self.ftan_threshold = max(self.ftan_threshold, ftan)
if self.hooked_location_moved and ang > math.radians(90.):
print 'Angle:', math.degrees(ang)
self.open_ang_exceed_count += 1
if self.open_ang_exceed_count > 2:
stop = 'opened mechanism through large angle: %.1f'%(math.degrees(ang))
else:
self.open_ang_exceed_count = 0
self.move_segway_lock.acquire()
if stop != '' and stop != 'reset timing':
self.segway_motion_tup = (0.0,0.0,0.0)
self.new_segway_command = True
self.move_segway_lock.release()
self.mech_time_list.append(time.time())
self.mech_x_list.append(ang)
self.f_rad_list.append(f_rad_mag)
self.f_tan_list.append(np.dot(f_vec, tangential_vec.A1))
self.tan_vec_list.append(tangential_vec_ts.A1.tolist())
self.rad_vec_list.append(force_vec_ts.A1.tolist())
return stop, q_eq
