def rotary_mechanism_configurations_from_global_poses(pts_2d, center):
st_pt = pts_2d[:,0]
cx = center[0,0]
cy = center[1,0]
start_angle = tr.angle_within_mod180(math.atan2(st_pt[1,0]-cy, st_pt[0,0]-cx) - math.radians(90)) #-90 for display
poses_list = []
for pos_idx in range(pts_2d.shape[1]):
end_pt = pts_2d[:, pos_idx]
end_angle = tr.angle_within_mod180(math.atan2(end_pt[1,0]-cy, end_pt[0,0]-cx) - math.radians(90))
poses_list.append(end_angle)
return poses_list
def rotary_mechanism_configurations_from_global_poses(pts_2d, center):
st_pt = pts_2d[:, 0]
cx = center[0, 0]
cy = center[1, 0]
start_angle = tr.angle_within_mod180(
math.atan2(st_pt[1, 0] - cy, st_pt[0, 0] - cx) -
math.radians(90)) #-90 for display
poses_list = []
for pos_idx in range(pts_2d.shape[1]):
end_pt = pts_2d[:, pos_idx]
end_angle = tr.angle_within_mod180(
math.atan2(end_pt[1, 0] - cy, end_pt[0, 0] - cx) -
math.radians(90))
poses_list.append(end_angle)
return poses_list
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 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 IK_kdl(self,frame, q_init):
nJnts = self.kdl_chains['nJnts']
ik_solver = self.kdl_chains['ik_p']
q = kdl.JntArray(nJnts)
if ik_solver.CartToJnt(q_init,frame,q)>=0:
for i in range(nJnts):
q[i] = tr.angle_within_mod180(q[i])
return q
else:
return None
def IK_kdl(self,frame, q_init):
nJnts = self.kdl_chains['nJnts']
ik_solver = self.kdl_chains['ik_p']
q = kdl.JntArray(nJnts)
if ik_solver.CartToJnt(q_init,frame,q)>=0:
for i in range(nJnts):
q[i] = tr.angle_within_mod180(q[i])
return q
else:
return None
def IK_kdl(self, arm, frame, q_init):
""" IK, returns jointArray (None if impossible)
frame - desired frame of the end effector
q_init - initial guess for the joint angles. (JntArray)
"""
nJnts = self.cody_kdl[arm]["nJnts"]
ik_solver = self.cody_kdl[arm]["ik_p"]
q = kdl.JntArray(nJnts)
if ik_solver.CartToJnt(q_init, frame, q) >= 0:
for i in range(nJnts):
q[i] = tr.angle_within_mod180(q[i])
return q
else:
if arm == "right_arm":
ik_solver = self.cody_kdl[arm]["ik_p_nolim"]
if ik_solver.CartToJnt(q_init, frame, q) >= 0:
for i in range(nJnts):
q[i] = tr.angle_within_mod180(q[i])
return q
print "Error: could not calculate inverse kinematics"
return None
def IK_kdl(self,arm,frame, q_init):
''' IK, returns jointArray (None if impossible)
frame - desired frame of the end effector
q_init - initial guess for the joint angles. (JntArray)
'''
nJnts = self.cody_kdl[arm]['nJnts']
ik_solver = self.cody_kdl[arm]['ik_p']
q = kdl.JntArray(nJnts)
if ik_solver.CartToJnt(q_init,frame,q)>=0:
for i in range(nJnts):
q[i] = tr.angle_within_mod180(q[i])
return q
else:
if arm == 'right_arm':
ik_solver = self.cody_kdl[arm]['ik_p_nolim']
if ik_solver.CartToJnt(q_init,frame,q)>=0:
for i in range(nJnts):
q[i] = tr.angle_within_mod180(q[i])
return q
print 'Error: could not calculate inverse kinematics'
return None
goalB.motion_plan_request.group_name = 'right_arm'
goalB.motion_plan_request.num_planning_attempts = 1
goalB.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goalB.motion_plan_request.planner_id = ''
goalB.planner_service_name = 'ompl_planning/plan_kinematic_path'
import roslib; roslib.load_manifest('darpa_m3')
import sandbox_advait.pr2_arms as pa
pr2_arms = pa.PR2Arms()
raw_input('Move arm to goal location and hit ENTER')
q = pr2_arms.get_joint_angles(0)
raw_input('Move arm to start location and hit ENTER')
q[6] = tr.angle_within_mod180(q[6])
q[4] = tr.angle_within_mod180(q[4])
for i in range(7):
jc = JointConstraint()
jc.joint_name = names[i]
jc.position = q[i]
jc.tolerance_below = 0.1
jc.tolerance_above = 0.1
goalB.motion_plan_request.goal_constraints.joint_constraints.append(jc)
move_arm.send_goal(goalB)
finished_within_time = move_arm.wait_for_result(rospy.Duration(200.0))
if not finished_within_time:
move_arm.cancel_goal()
rospy.loginfo("Timed out achieving goal A")
goalB.motion_plan_request.group_name = 'right_arm'
goalB.motion_plan_request.num_planning_attempts = 1
goalB.motion_plan_request.allowed_planning_time = rospy.Duration(5.0)
goalB.motion_plan_request.planner_id = ''
goalB.planner_service_name = 'ompl_planning/plan_kinematic_path'
import roslib
roslib.load_manifest('darpa_m3')
import sandbox_advait.pr2_arms as pa
pr2_arms = pa.PR2Arms()
raw_input('Move arm to goal location and hit ENTER')
q = pr2_arms.get_joint_angles(0)
raw_input('Move arm to start location and hit ENTER')
q[6] = tr.angle_within_mod180(q[6])
q[4] = tr.angle_within_mod180(q[4])
for i in range(7):
jc = JointConstraint()
jc.joint_name = names[i]
jc.position = q[i]
jc.tolerance_below = 0.1
jc.tolerance_above = 0.1
goalB.motion_plan_request.goal_constraints.joint_constraints.append(jc)
move_arm.send_goal(goalB)
finished_within_time = move_arm.wait_for_result(rospy.Duration(200.0))
if not finished_within_time:
move_arm.cancel_goal()
rospy.loginfo("Timed out achieving goal A")
sturm_file.write(" ".join(map(str, p)))
sturm_file.write('\n')
sturm_file.write('\n')
sturm_file.close()
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,
x_guess,
y_guess,
pts_2d,
method='fmin_bfgs',
verbose=False)
print 'rad, cx, cy:', rad, cx, cy
c_ts = np.matrix([cx, cy, 0.]).T
start_angle = tr.angle_within_mod180(
math.atan2(pts_2d[1, 0] - cy, pts_2d[0, 0] - cx) - math.pi / 2)
end_angle = tr.angle_within_mod180(
math.atan2(pts_2d[1, -1] - cy, pts_2d[0, -1] - cx) - math.pi / 2)
mpu.plot_circle(cx,
cy,
rad,
start_angle,
end_angle,
label='Actual\_opt',
color='r')
if opt.icra_presentation_plot:
mpu.set_figure_size(30, 20)
rad = 1.0
x_guess = pts_2d[0, 0]
y_guess = pts_2d[1, 0] - rad
sturm_pts = cartesian_force_clean.p_list
print 'len(sturm_pts):', len(sturm_pts)
print 'len(pts_list):', len(pts_list)
for i,p in enumerate(sturm_pts[1:]):
sturm_file.write(" ".join(map(str,p)))
sturm_file.write('\n')
sturm_file.write('\n')
sturm_file.close()
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
c_ts = np.matrix([cx, cy, 0.]).T
start_angle = tr.angle_within_mod180(math.atan2(pts_2d[0,1]-cy,
pts_2d[0,0]-cx) - math.pi/2)
end_angle = tr.angle_within_mod180(math.atan2(pts_2d[-1,1]-cy,
pts_2d[-1,0]-cx) - math.pi/2)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='Actual\_opt', color='r')
if opt.icra_presentation_plot:
mpu.set_figure_size(30,20)
rad = 1.0
x_guess = pts_2d[0,0]
y_guess = pts_2d[1,0] - rad
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
rot_mat = tr.Rz(angle)[0:2,0:2]
translation_mat = np.matrix([cx,cy]).T
robot_width,robot_length = 0.1,0.2
robot_x_list = [-robot_width/2,-robot_width/2,robot_width/2,robot_width/2,-robot_width/2]
robot_y_list = [-robot_length/2,robot_length/2,robot_length/2,-robot_length/2,-robot_length/2]
robot_mat = rot_mat*(np.matrix([robot_x_list,robot_y_list]) - translation_mat)
mpu.plot_yx(robot_mat[1,:].A1,robot_mat[0,:].A1,linewidth=2,scatter_size=0,
color='k',label='torso')
pts2d_actual = (np.matrix(actual_cartesian.p_list).T)[0:2]
pts2d_actual_t = rot_mat *(pts2d_actual - translation_mat)
mpu.plot_yx(pts2d_actual_t[1,:].A1,pts2d_actual_t[0,:].A1,scatter_size=20,label='FK')
end_pt = pts2d_actual_t[:,-1]
end_angle = tr.angle_within_mod180(math.atan2(end_pt[1,0],end_pt[0,0])-math.radians(90))
mpu.plot_circle(0,0,rad,0.,end_angle,label='Actual_opt',color='r')
mpu.plot_radii(0,0,rad,0.,end_angle,interval=math.radians(15),color='r')
pl.legend(loc='best')
pl.axis('equal')
if not(expt_plot):
str_parts = fname.split('.')
fig_name = str_parts[0]+'_robot_pose.png'
pl.savefig(fig_name)
pl.figure()
else:
pl.subplot(232)
pl.text(0.1,0.15,d['info'])
print 'len(sturm_pts):', len(sturm_pts)
print 'len(pts_list):', len(pts_list)
for i,p in enumerate(sturm_pts[1:]):
sturm_file.write(" ".join(map(str,p)))
sturm_file.write('\n')
sturm_file.write('\n')
sturm_file.close()
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
print 'rad, cx, cy:', rad, cx, cy
c_ts = np.matrix([cx, cy, 0.]).T
start_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,0]-cy,
pts_2d[0,0]-cx) - math.pi/2)
end_angle = tr.angle_within_mod180(math.atan2(pts_2d[1,-1]-cy,
pts_2d[0,-1]-cx) - math.pi/2)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='Actual\_opt', color='r')
if opt.icra_presentation_plot:
mpu.set_figure_size(30,20)
rad = 1.0
x_guess = pts_2d[0,0]
y_guess = pts_2d[1,0] - rad
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
sturm_pts = cartesian_force_clean.p_list
print 'len(sturm_pts):', len(sturm_pts)
print 'len(pts_list):', len(pts_list)
for i,p in enumerate(sturm_pts[1:]):
sturm_file.write(" ".join(map(str,p)))
sturm_file.write('\n')
sturm_file.write('\n')
sturm_file.close()
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
c_ts = np.matrix([cx, cy, 0.]).T
start_angle = tr.angle_within_mod180(math.atan2(pts_2d[0,1]-cy,
pts_2d[0,0]-cx) - math.pi/2)
end_angle = tr.angle_within_mod180(math.atan2(pts_2d[-1,1]-cy,
pts_2d[-1,0]-cx) - math.pi/2)
mpu.plot_circle(cx, cy, rad, start_angle, end_angle,
label='Actual\_opt', color='r')
if opt.icra_presentation_plot:
mpu.set_figure_size(30,20)
rad = 1.0
x_guess = pts_2d[0,0]
y_guess = pts_2d[1,0] - rad
rad_guess = rad
rad, cx, cy = fit_circle(rad_guess,x_guess,y_guess,pts_2d,
method='fmin_bfgs',verbose=False)
