def circle_estimator(self):
self.run_fit_circle_thread = True
print 'Starting the circle estimating thread.'
while self.run_fit_circle_thread:
self.fit_circle_lock.acquire()
if len(self.cartesian_pts_list) == 0:
self.fit_circle_lock.release()
continue
pts_2d = (np.matrix(self.cartesian_pts_list).T)[0:2, :]
self.fit_circle_lock.release()
rad = self.rad_guess
start_pos = self.firenze.FK('right_arm',
self.pull_trajectory.q_list[0])
rad, cx, cy = at.fit_circle(rad,
start_pos[0, 0],
start_pos[1, 0] - rad,
pts_2d,
method='fmin_bfgs',
verbose=False)
rad = ut.bound(rad, 3.0, 0.1)
self.fit_circle_lock.acquire()
self.cx = cx
self.cy = cy
# self.rad = rad
self.fit_circle_lock.release()
print 'Ended the circle estimating thread.'
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 equi_pt_generator_control_radial_force(self):
self.log_state()
q_eq = self.update_eq_point(self.eq_motion_vec, 0.01)
stop = self.common_stopping_conditions()
wrist_force = self.firenze.get_wrist_force('right_arm',
base_frame=True)
mag = np.linalg.norm(wrist_force)
start_pos = self.firenze.FK('right_arm',
self.pull_trajectory.q_list[0])
curr_pos = self.firenze.FK('right_arm',
self.pull_trajectory.q_list[-1])
if (start_pos[0, 0] -
curr_pos[0, 0]) > 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.piecewise_force_threshold = ut.bound(mag + 5., 0., 80.)
self.fit_circle_lock.acquire()
self.cartesian_pts_list.append(curr_pos.A1.tolist())
self.fit_circle_lock.release()
# find tangential direction.
radial_vec = curr_pos[0:2] - np.matrix([self.cx, self.cy]).T
radial_vec = radial_vec / np.linalg.norm(radial_vec)
tan_x, tan_y = -radial_vec[1, 0], radial_vec[0, 0]
if tan_x > 0.:
tan_x = -tan_x
tan_y = -tan_y
self.eq_motion_vec = np.matrix([tan_x, tan_y, 0.]).T
f_vec = -1 * np.array([wrist_force[0, 0], wrist_force[1, 0]])
f_rad_mag = np.dot(f_vec, radial_vec.A1)
#if f_rad_mag>10.:
if f_rad_mag > 5.:
self.eq_motion_vec[
0:2] -= radial_vec / 2. * self.hook_maintain_dist_plane / 0.05
else:
self.eq_motion_vec[
0:2] += radial_vec / 2. * self.hook_maintain_dist_plane / 0.05
self.prev_force_mag = mag
return stop, q_eq
def reposition_robot(self,hook_location,turn_angle,hook_angle,position_number=2):
h = self.workspace_dict[hook_angle]['height']
max_z_height = 1.3
min_z_height = 0.5
h_desired = self.z.get_position_meters()+hook_location[2,0]-h
print 'h_desired:',h_desired
print 'h:',h
h_achieve = ut.bound(h_desired,max_z_height,min_z_height)
h_err = h_desired-h_achieve
h = h+h_err
print 'h_achieve:',h_achieve
wkd = self.workspace_dict[hook_angle]['pts']
k = wkd.keys()
k_idx = np.argmin(np.abs(np.array(k)-h))
wrkspc_pts = wkd[k[k_idx]]
# good_location = np.matrix([0.45,-0.35,h]).T
good_location = wrkspc_pts.mean(1)
good_location = np.matrix(good_location.A1.tolist()+[h]).T
if position_number == 1:
good_location[0,0] += 0.0
good_location[1,0] -= 0.1
elif position_number == 2:
good_location[0,0] += 0.0
good_location[1,0] -= 0.0
elif position_number == 3:
good_location[0,0] += 0.0
good_location[1,0] += 0.1
else:
good_location[0,0] -= 0.1
good_location[1,0] -= 0.0
good_location = mcf.mecanumTglobal(mcf.globalTtorso(good_location))
hook_location = mcf.mecanumTglobal(mcf.globalTtorso(hook_location))
v = tr.Rz(-turn_angle)*good_location
move_vector = hook_location - v
pose1 = self.segway_pose.get_pose()
self.segway_command_node.go_xya_pos_local(move_vector[0,0],move_vector[1,0],
turn_angle,blocking=False)
self.z.torque_move_position(h_achieve)
print 'before waiting for segway to stop moving.'
self.segway_command_node.wait_for_tolerance_pos(0.03,0.03,math.radians(4))
print 'after segway has stopped moving.'
pose2 = self.segway_pose.get_pose()
hl_new = vop.transform_pts_local(hook_location[0:2,:],pose1,pose2)
h_err = h_desired-self.z.get_position_meters()
g = np.matrix(hl_new.A1.tolist()+[good_location[2,0]+h_err]).T
g = mcf.torsoTglobal(mcf.globalTmecanum(g))
angle_turned = pose2[2]-pose1[2]
angle_error = tr.angle_within_mod180(turn_angle-angle_turned)
self.segway_pose.set_origin() # re-origining for the manipulation behaviors
return g, angle_error
def equi_pt_generator_control_radial_force(self):
self.log_state()
q_eq = self.update_eq_point(self.eq_motion_vec,0.01)
stop = self.common_stopping_conditions()
wrist_force = self.firenze.get_wrist_force('right_arm',base_frame=True)
mag = np.linalg.norm(wrist_force)
start_pos = self.firenze.FK('right_arm',self.pull_trajectory.q_list[0])
curr_pos = self.firenze.FK('right_arm',self.pull_trajectory.q_list[-1])
if (start_pos[0,0]-curr_pos[0,0])>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.piecewise_force_threshold = ut.bound(mag+5.,0.,80.)
self.fit_circle_lock.acquire()
self.cartesian_pts_list.append(curr_pos.A1.tolist())
self.fit_circle_lock.release()
# find tangential direction.
radial_vec = curr_pos[0:2]-np.matrix([self.cx,self.cy]).T
radial_vec = radial_vec/np.linalg.norm(radial_vec)
tan_x,tan_y = -radial_vec[1,0],radial_vec[0,0]
if tan_x >0.:
tan_x = -tan_x
tan_y = -tan_y
self.eq_motion_vec = np.matrix([tan_x,tan_y,0.]).T
f_vec = -1*np.array([wrist_force[0,0],wrist_force[1,0]])
f_rad_mag = np.dot(f_vec,radial_vec.A1)
#if f_rad_mag>10.:
if f_rad_mag>5.:
self.eq_motion_vec[0:2] -= radial_vec/2. * self.hook_maintain_dist_plane/0.05
else:
self.eq_motion_vec[0:2] += radial_vec/2. * self.hook_maintain_dist_plane/0.05
self.prev_force_mag = mag
return stop,q_eq
def move(self, events):
if len(events) > 0:
for event in events:
currselect = np.nonzero( self.selected )[0]
if event.type == pyc.KEYDOWN:
if event.key == pyc.K_DOWN:
self.selected[currselect] = 0
self.selected[ ut.bound(currselect + 1, 0, self.selected.shape[0]-1) ] = 1
if event.key == pyc.K_UP:
self.selected[currselect] = 0
self.selected[ ut.bound(currselect - 1, 0, self.selected.shape[0]-1) ] = 1
if event.key == pyc.K_LEFT:
self.vals[currselect] -= self.dels[currselect]
if event.key == pyc.K_RIGHT:
self.vals[currselect] += self.dels[currselect]
if event.key == pyc.K_SPACE:
self.display_laser = not self.display_laser
if event.key == pyc.K_RETURN:
self.display3d()
return events
def move(self, events):
if len(events) > 0:
for event in events:
currselect = np.nonzero(self.selected)[0]
if event.type == pyc.KEYDOWN:
if event.key == pyc.K_DOWN:
self.selected[currselect] = 0
self.selected[ut.bound(currselect + 1, 0,
self.selected.shape[0] - 1)] = 1
if event.key == pyc.K_UP:
self.selected[currselect] = 0
self.selected[ut.bound(currselect - 1, 0,
self.selected.shape[0] - 1)] = 1
if event.key == pyc.K_LEFT:
self.vals[currselect] -= self.dels[currselect]
if event.key == pyc.K_RIGHT:
self.vals[currselect] += self.dels[currselect]
if event.key == pyc.K_SPACE:
self.display_laser = not self.display_laser
if event.key == pyc.K_RETURN:
self.display3d()
return events
def equi_pt_generator_line(self):
self.log_state()
#q_eq = self.update_eq_point(self.eq_motion_vec,0.005)
q_eq = self.update_eq_point(self.eq_motion_vec,0.010)
stop = self.common_stopping_conditions()
wrist_force = self.firenze.get_wrist_force('right_arm',base_frame=True)
mag = np.linalg.norm(wrist_force)
start_pos = self.firenze.FK('right_arm',self.pull_trajectory.q_list[0])
curr_pos = self.firenze.FK('right_arm',self.pull_trajectory.q_list[-1])
if (start_pos[0,0]-curr_pos[0,0])>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+15.,20.,80.)
self.prev_force_mag = mag
return stop,q_eq
def equi_pt_generator_line(self):
self.log_state()
#q_eq = self.update_eq_point(self.eq_motion_vec,0.005)
q_eq = self.update_eq_point(self.eq_motion_vec, 0.010)
stop = self.common_stopping_conditions()
wrist_force = self.firenze.get_wrist_force('right_arm',
base_frame=True)
mag = np.linalg.norm(wrist_force)
start_pos = self.firenze.FK('right_arm',
self.pull_trajectory.q_list[0])
curr_pos = self.firenze.FK('right_arm',
self.pull_trajectory.q_list[-1])
if (start_pos[0, 0] -
curr_pos[0, 0]) > 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 + 15., 20., 80.)
self.prev_force_mag = mag
return stop, q_eq
def go_height(self, z, function=None, down_torque=0, up_torque=200):
if self.robot != 'El-E':
raise RuntimeError(
'This function is unemplemented on robot\'s other than El-E')
#z = mut.bound(z,0.0, .89)
z = ut.bound(z, 0.0, .89)
initial_direction = None
#if not ut.approx_equal(z, self.broadcaster.pose):
if not ut.approx_equal(z, self.get_position_meters()):
#if z > self.broadcaster.pose:
if z > self.get_position_meters():
self.zenith(torque=up_torque)
initial_direction = 'up'
else:
self.zenith(torque=down_torque)
initial_direction = 'down'
else:
return
there = False
#while not ut.approx_equal(z, self.broadcaster.pose):
while not ut.approx_equal(z, self.get_position_meters()):
if function != None:
#ret = function(self.broadcaster.pose)
ret = function(self.get_position_meters())
if ret:
break
#if initial_direction == 'up' and self.broadcaster.pose > z:
if initial_direction == 'up' and self.get_position_meters() > z:
break
if initial_direction == 'down' and self.get_position_meters() < z:
break
self.estop()
def go_height(self, z, function=None, down_torque=0, up_torque=200):
if self.robot != 'El-E':
raise RuntimeError(
'This function is unemplemented on robot\'s other than El-E')
#z = mut.bound(z,0.0, .89)
z = ut.bound(z,0.0, .89)
initial_direction = None
#if not ut.approx_equal(z, self.broadcaster.pose):
if not ut.approx_equal(z, self.get_position_meters()):
#if z > self.broadcaster.pose:
if z > self.get_position_meters():
self.zenith(torque = up_torque)
initial_direction = 'up'
else:
self.zenith(torque = down_torque)
initial_direction = 'down'
else:
return
there = False
#while not ut.approx_equal(z, self.broadcaster.pose):
while not ut.approx_equal(z, self.get_position_meters()):
if function != None:
#ret = function(self.broadcaster.pose)
ret = function(self.get_position_meters())
if ret:
break
#if initial_direction == 'up' and self.broadcaster.pose > z:
if initial_direction == 'up' and self.get_position_meters() > z:
break
if initial_direction == 'down' and self.get_position_meters() < z:
break
self.estop()
def circle_estimator(self):
self.run_fit_circle_thread = True
print 'Starting the circle estimating thread.'
while self.run_fit_circle_thread:
self.fit_circle_lock.acquire()
if len(self.cartesian_pts_list)==0:
self.fit_circle_lock.release()
continue
pts_2d = (np.matrix(self.cartesian_pts_list).T)[0:2,:]
self.fit_circle_lock.release()
rad = self.rad_guess
start_pos = self.firenze.FK('right_arm',self.pull_trajectory.q_list[0])
rad,cx,cy = at.fit_circle(rad,start_pos[0,0],start_pos[1,0]-rad,pts_2d,method='fmin_bfgs',verbose=False)
rad = ut.bound(rad,3.0,0.1)
self.fit_circle_lock.acquire()
self.cx = cx
self.cy = cy
# self.rad = rad
self.fit_circle_lock.release()
print 'Ended the circle estimating thread.'
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
print "joystick error"
rospy.signal_shutdown()
# if zenither_flag and (move_zenither_flag == False):
# z.estop()
#send segway commands
if connected:
xvel = x*max_xvel
# xvel_history[0:(len-1)] = xvel_history[1:len]
# xvel_history[(len-1)] = xvel
# xvel = np.mean(xvel_history)
yvel = y*max_yvel
avel = a*max_avel
vel_vec = np.matrix([xvel,yvel]).T
vel_mag = np.linalg.norm(vel_vec)
speed = ut.bound(vel_mag,max_speed,0.)
if speed >= 0.05:
vel_vec = vel_vec*speed/vel_mag
xvel,yvel = vel_vec[0,0],vel_vec[1,0]
cmd_node.set_velocity(xvel,yvel,avel)
print '*******xvel=',xvel,'; avel=',avel
# stop the segway
cmd_node.set_velocity(0.,0.,0.)
def cep_gen_control_radial_force(self, arm, cep, cep_vel):
self.log_state(arm)
if self.started_pulling_on_handle == False:
cep_vel = 0.02
#step_size = 0.01 * cep_vel
step_size = 0.1 * cep_vel # 0.1 is the time interval between calls to the equi_generator function (see pull)
stop = self.common_stopping_conditions()
wrist_force = self.robot.get_wrist_force(arm, base_frame=True)
mag = np.linalg.norm(wrist_force)
curr_pos, _ = self.robot.get_ee_jtt(arm)
if len(self.ee_list)>1:
start_pos = np.matrix(self.ee_list[0]).T
else:
start_pos = curr_pos
#mechanism kinematics.
if self.started_pulling_on_handle:
self.mech_traj_pub.publish(Point32(curr_pos[0,0],
curr_pos[1,0], curr_pos[2,0]))
self.fit_circle_lock.acquire()
rad = self.rad
cx_start, cy_start = self.cx_start, self.cy_start
cz_start = self.cz_start
self.fit_circle_lock.release()
cx, cy = cx_start, cy_start
cz = cz_start
print 'cx, cy, r:', cx, cy, rad
radial_vec = curr_pos - np.matrix([cx,cy,cz]).T
radial_vec = radial_vec/np.linalg.norm(radial_vec)
if cy_start < start_pos[1,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
tangential_vec_ts = tangential_vec
radial_vec_ts = radial_vec
force_vec_ts = force_vec
if arm == 'right_arm' or arm == 0:
if force_vec_ts[1,0] < 0.: # only allowing force to the left
force_vec_ts = -force_vec_ts
else:
if force_vec_ts[1,0] > 0.: # only allowing force to the right
force_vec_ts = -force_vec_ts
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-4.
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)
radial_motion_vec = force_vec * radial_motion_mag
print 'tangential_vec:', tangential_vec.A1
eq_motion_vec = copy.copy(tangential_vec)
eq_motion_vec += radial_motion_vec
self.prev_force_mag = mag
if self.init_tangent_vector == None or self.started_pulling_on_handle == False:
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)
if np.isnan(ang):
ang = 0.
tangential_vec = tangential_vec / np.linalg.norm(tangential_vec) # paranoia abot vectors not being unit vectors.
dist_moved = np.dot((curr_pos - start_pos).A1, tangential_vec_ts.A1)
ftan = abs(np.dot(wrist_force.A1, tangential_vec.A1))
self.ft.tangential_force.append(ftan)
self.ft.radial_force.append(f_rad_mag)
if self.ft.type == 'rotary':
self.ft.configuration.append(ang)
else: # drawer
print 'dist_moved:', dist_moved
self.ft.configuration.append(dist_moved)
if self.started_pulling_on_handle:
self.force_traj_pub.publish(self.ft)
# if self.started_pulling_on_handle == False:
# ftan_pull_test = -np.dot(wrist_force.A1, tangential_vec.A1)
# print 'ftan_pull_test:', ftan_pull_test
# if ftan_pull_test > 5.:
# self.started_pulling_on_handle_count += 1
# else:
# self.started_pulling_on_handle_count = 0
# self.init_log() # reset logs until started pulling on the handle.
# self.init_tangent_vector = None
#
# if self.started_pulling_on_handle_count > 1:
# self.started_pulling_on_handle = True
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 = 1.2 * self.ftan_threshold + 20.
if self.hooked_location_moved:
if abs(tangential_vec_ts[2,0]) < 0.2 and ftan > self.ftan_threshold:
stop = 'ftan threshold exceed: %f'%ftan
else:
self.ftan_threshold = max(self.ftan_threshold, ftan)
if self.hooked_location_moved and ang > math.radians(85.):
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
cep_t = cep + eq_motion_vec * step_size
cep[0,0] = cep_t[0,0]
cep[1,0] = cep_t[1,0]
cep[2,0] = cep_t[2,0]
print 'CEP:', cep.A1
stop = stop + self.stopping_string
return stop, (cep, None)
def cep_gen_control_radial_force(self, arm, cep, cep_vel):
self.log_state(arm)
if self.started_pulling_on_handle == False:
cep_vel = 0.02
#step_size = 0.01 * cep_vel
step_size = 0.1 * cep_vel # 0.1 is the time interval between calls to the equi_generator function (see pull)
stop = self.common_stopping_conditions()
wrist_force = self.robot.get_wrist_force(arm, base_frame=True)
mag = np.linalg.norm(wrist_force)
curr_pos, _ = self.robot.get_ee_jtt(arm)
if len(self.ee_list) > 1:
start_pos = np.matrix(self.ee_list[0]).T
else:
start_pos = curr_pos
#mechanism kinematics.
if self.started_pulling_on_handle:
self.mech_traj_pub.publish(
Point32(curr_pos[0, 0], curr_pos[1, 0], curr_pos[2, 0]))
self.fit_circle_lock.acquire()
rad = self.rad
cx_start, cy_start = self.cx_start, self.cy_start
cz_start = self.cz_start
self.fit_circle_lock.release()
cx, cy = cx_start, cy_start
cz = cz_start
print 'cx, cy, r:', cx, cy, rad
radial_vec = curr_pos - np.matrix([cx, cy, cz]).T
radial_vec = radial_vec / np.linalg.norm(radial_vec)
if cy_start < start_pos[1, 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
tangential_vec_ts = tangential_vec
radial_vec_ts = radial_vec
force_vec_ts = force_vec
if arm == 'right_arm' or arm == 0:
if force_vec_ts[1, 0] < 0.: # only allowing force to the left
force_vec_ts = -force_vec_ts
else:
if force_vec_ts[1, 0] > 0.: # only allowing force to the right
force_vec_ts = -force_vec_ts
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 - 4.
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)
radial_motion_vec = force_vec * radial_motion_mag
print 'tangential_vec:', tangential_vec.A1
eq_motion_vec = copy.copy(tangential_vec)
eq_motion_vec += radial_motion_vec
self.prev_force_mag = mag
if self.init_tangent_vector == None or self.started_pulling_on_handle == False:
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)
if np.isnan(ang):
ang = 0.
tangential_vec = tangential_vec / np.linalg.norm(
tangential_vec) # paranoia abot vectors not being unit vectors.
dist_moved = np.dot((curr_pos - start_pos).A1, tangential_vec_ts.A1)
ftan = abs(np.dot(wrist_force.A1, tangential_vec.A1))
self.ft.tangential_force.append(ftan)
self.ft.radial_force.append(f_rad_mag)
if self.ft.type == 'rotary':
self.ft.configuration.append(ang)
else: # drawer
print 'dist_moved:', dist_moved
self.ft.configuration.append(dist_moved)
if self.started_pulling_on_handle:
self.force_traj_pub.publish(self.ft)
# if self.started_pulling_on_handle == False:
# ftan_pull_test = -np.dot(wrist_force.A1, tangential_vec.A1)
# print 'ftan_pull_test:', ftan_pull_test
# if ftan_pull_test > 5.:
# self.started_pulling_on_handle_count += 1
# else:
# self.started_pulling_on_handle_count = 0
# self.init_log() # reset logs until started pulling on the handle.
# self.init_tangent_vector = None
#
# if self.started_pulling_on_handle_count > 1:
# self.started_pulling_on_handle = True
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 = 1.2 * self.ftan_threshold + 20.
if self.hooked_location_moved:
if abs(tangential_vec_ts[2,
0]) < 0.2 and ftan > self.ftan_threshold:
stop = 'ftan threshold exceed: %f' % ftan
else:
self.ftan_threshold = max(self.ftan_threshold, ftan)
if self.hooked_location_moved and ang > math.radians(85.):
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
cep_t = cep + eq_motion_vec * step_size
cep[0, 0] = cep_t[0, 0]
cep[1, 0] = cep_t[1, 0]
cep[2, 0] = cep_t[2, 0]
print 'CEP:', cep.A1
stop = stop + self.stopping_string
return stop, (cep, None)
