def full_ik_check(self, ps):
#Check goal as given, if reachable, return true
req = self.form_ik_request(ps)
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
self.dist = pu.calc_dist(self.curr_pose(), ps)
return (True, ik_goal, None)
#Check goal with vertical torso movement, if works, return true
(torso_succeeded, torso_move) = self.check_torso(req)
self.move_torso(self.torso_state.actual.positions[0] + torso_move)
duration = max(4, 85 * abs(torso_move))
if torso_succeeded:
ik_goal = self.ik_pose_proxy(req)
self.dist = pu.calc_dist(self.curr_pose(), ps)
if ik_goal.error_code.val == 1:
return (True, ik_goal, duration)
else:
print "Reported Succeeded, then really failed: \r\n", ik_goal
#Check goal incrementally retreating hand pose, if works, return true
pct = 0.98
curr_pos = self.curr_pose().pose.position
dx = req.ik_request.pose_stamped.pose.position.x - curr_pos.x
dy = req.ik_request.pose_stamped.pose.position.y - curr_pos.y
while pct > 0.01:
#print "Percent: %s" %pct
req.ik_request.pose_stamped.pose.position.x = curr_pos.x + pct * dx
req.ik_request.pose_stamped.pose.position.y = curr_pos.y + pct * dy
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
rospy.loginfo("Succeeded PARTIALLY (%s) with torso" % pct)
return (True, ik_goal, duration)
else:
pct -= 0.02
#Nothing worked, report failure, return false
rospy.loginfo("IK Failed: Error Code %s" % str(ik_goal.error_code))
rospy.loginfo("Reached as far as possible")
self.log_out.publish(data="Cannot reach farther in this direction.")
return (False, ik_goal, duration)
def full_ik_check(self, ps):
#Check goal as given, if reachable, return true
req = self.form_ik_request(ps)
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
self.dist = pu.calc_dist(self.curr_pose(), ps)
return (True, ik_goal, None)
#Check goal with vertical torso movement, if works, return true
(torso_succeeded, torso_move) = self.check_torso(req)
self.move_torso(self.torso_state.actual.positions[0]+torso_move)
duration = max(4,85*abs(torso_move))
if torso_succeeded:
ik_goal = self.ik_pose_proxy(req)
self.dist = pu.calc_dist(self.curr_pose(), ps)
if ik_goal.error_code.val ==1:
return (True, ik_goal, duration)
else:
print "Reported Succeeded, then really failed: \r\n", ik_goal
#Check goal incrementally retreating hand pose, if works, return true
pct = 0.98
curr_pos = self.curr_pose().pose.position
dx = req.ik_request.pose_stamped.pose.position.x - curr_pos.x
dy = req.ik_request.pose_stamped.pose.position.y - curr_pos.y
while pct > 0.01:
#print "Percent: %s" %pct
req.ik_request.pose_stamped.pose.position.x = curr_pos.x + pct*dx
req.ik_request.pose_stamped.pose.position.y = curr_pos.y + pct*dy
ik_goal = self.ik_pose_proxy(req)
if ik_goal.error_code.val == 1:
rospy.loginfo("Succeeded PARTIALLY (%s) with torso" %pct)
return (True, ik_goal, duration)
else:
pct -= 0.02
#Nothing worked, report failure, return false
rospy.loginfo("IK Failed: Error Code %s" %str(ik_goal.error_code))
rospy.loginfo("Reached as far as possible")
self.log_out.publish(data="Cannot reach farther in this direction.")
return (False, ik_goal, duration)
def build_trajectory(self,
finish,
start=None,
ik_space=0.005,
duration=None,
tot_points=None):
if start == None: # if given one pose, use current position as start, else, assume (start, finish)
start = self.curr_pose()
#TODO: USE TF TO BASE DISTANCE ON TOOL_FRAME MOVEMENT DISTANCE, NOT WRIST_ROLL_LINK
# Based upon distance to travel, determine the number of intermediate steps required
# find linear increments of position.
dist = pu.calc_dist(start, finish) #Total distance to travel
ik_steps = int(round(dist / ik_space) + 1.)
print "IK Steps: %s" % ik_steps
if tot_points is None:
tot_points = 1500 * dist
if duration is None:
duration = dist * 120
ik_fracs = np.linspace(
0, 1, ik_steps
) #A list of fractional positions along course to evaluate ik
ang_fracs = np.linspace(0, 1, tot_points)
x_gap = finish.pose.position.x - start.pose.position.x
y_gap = finish.pose.position.y - start.pose.position.y
z_gap = finish.pose.position.z - start.pose.position.z
qs = [
start.pose.orientation.x, start.pose.orientation.y,
start.pose.orientation.z, start.pose.orientation.w
]
qf = [
finish.pose.orientation.x, finish.pose.orientation.y,
finish.pose.orientation.z, finish.pose.orientation.w
]
#For each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
steps = [PoseStamped() for i in range(len(ik_fracs))]
for i, frac in enumerate(ik_fracs):
steps[i].header.stamp = rospy.Time.now()
steps[i].header.frame_id = start.header.frame_id
steps[i].pose.position.x = start.pose.position.x + x_gap * frac
steps[i].pose.position.y = start.pose.position.y + y_gap * frac
steps[i].pose.position.z = start.pose.position.z + z_gap * frac
new_q = transformations.quaternion_slerp(qs, qf, frac)
steps[i].pose.orientation.x = new_q[0]
steps[i].pose.orientation.y = new_q[1]
steps[i].pose.orientation.z = new_q[2]
steps[i].pose.orientation.w = new_q[3]
rospy.loginfo("Planning straight-line path, please wait")
self.log_out.publish(data="Planning straight-line path, please wait")
#For each pose in trajectory, find ik angles
#Find initial ik for seeding
req = self.form_ik_request(steps[0])
ik_goal = self.ik_pose_proxy(req)
seed = ik_goal.solution.joint_state.position
ik_points = [[0] * 7 for i in range(len(ik_fracs))]
for i, p in enumerate(steps):
request = self.form_ik_request(p)
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
ik_points[i] = ik_goal.solution.joint_state.position
seed = ik_goal.solution.joint_state.position # seed the next ik w/previous points results
ik_points = np.array(ik_points)
# Linearly interpolate angles 10 times between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path. Used to maintain large number of trajectory points without running IK on every one.
angle_points = np.zeros((7, tot_points))
for i in xrange(7):
angle_points[i, :] = np.interp(ang_fracs, ik_fracs, ik_points[:,
i])
#Build Joint Trajectory from angle positions
traj = JointTrajectory()
traj.header.frame_id = steps[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
#print 'angle_points', len(angle_points[0]), angle_points
for i in range(len(angle_points[0])):
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = angle_points[:, i]
traj.points[i].velocities = [0] * 7
traj.points[i].time_from_start = rospy.Duration(ang_fracs[i] *
duration)
return traj
def hand_frame_move(self, x, y=0, z=0, duration=None):
cp = self.curr_pose()
newpose = pu.pose_relative_trans(cp, x, y, z)
self.dist = pu.calc_dist(cp, newpose)
return newpose
def build_trajectory(self, finish, start=None, ik_space=0.005,
duration=None, tot_points=None):
if start == None: # if given one pose, use current position as start, else, assume (start, finish)
start = self.curr_pose()
#TODO: USE TF TO BASE DISTANCE ON TOOL_FRAME MOVEMENT DISTANCE, NOT WRIST_ROLL_LINK
# Based upon distance to travel, determine the number of intermediate steps required
# find linear increments of position.
dist = pu.calc_dist(start, finish) #Total distance to travel
ik_steps = int(round(dist/ik_space)+1.)
print "IK Steps: %s" %ik_steps
if tot_points is None:
tot_points = 1500*dist
if duration is None:
duration = dist*120
ik_fracs = np.linspace(0, 1, ik_steps) #A list of fractional positions along course to evaluate ik
ang_fracs = np.linspace(0,1, tot_points)
x_gap = finish.pose.position.x - start.pose.position.x
y_gap = finish.pose.position.y - start.pose.position.y
z_gap = finish.pose.position.z - start.pose.position.z
qs = [start.pose.orientation.x, start.pose.orientation.y,
start.pose.orientation.z, start.pose.orientation.w]
qf = [finish.pose.orientation.x, finish.pose.orientation.y,
finish.pose.orientation.z, finish.pose.orientation.w]
#For each step between start and finish, find a complete pose (linear position changes, and quaternion slerp)
steps = [PoseStamped() for i in range(len(ik_fracs))]
for i,frac in enumerate(ik_fracs):
steps[i].header.stamp = rospy.Time.now()
steps[i].header.frame_id = start.header.frame_id
steps[i].pose.position.x = start.pose.position.x + x_gap*frac
steps[i].pose.position.y = start.pose.position.y + y_gap*frac
steps[i].pose.position.z = start.pose.position.z + z_gap*frac
new_q = transformations.quaternion_slerp(qs,qf,frac)
steps[i].pose.orientation.x = new_q[0]
steps[i].pose.orientation.y = new_q[1]
steps[i].pose.orientation.z = new_q[2]
steps[i].pose.orientation.w = new_q[3]
rospy.loginfo("Planning straight-line path, please wait")
self.log_out.publish(data="Planning straight-line path, please wait")
#For each pose in trajectory, find ik angles
#Find initial ik for seeding
req = self.form_ik_request(steps[0])
ik_goal = self.ik_pose_proxy(req)
seed = ik_goal.solution.joint_state.position
ik_points = [[0]*7 for i in range(len(ik_fracs))]
for i, p in enumerate(steps):
request = self.form_ik_request(p)
request.ik_request.ik_seed_state.joint_state.position = seed
ik_goal = self.ik_pose_proxy(request)
ik_points[i] = ik_goal.solution.joint_state.position
seed = ik_goal.solution.joint_state.position # seed the next ik w/previous points results
ik_points = np.array(ik_points)
# Linearly interpolate angles 10 times between ik-defined points (non-linear in cartesian space, but this is reduced from dense ik sampling along linear path. Used to maintain large number of trajectory points without running IK on every one.
angle_points = np.zeros((7, tot_points))
for i in xrange(7):
angle_points[i,:] = np.interp(ang_fracs, ik_fracs, ik_points[:,i])
#Build Joint Trajectory from angle positions
traj = JointTrajectory()
traj.header.frame_id = steps[0].header.frame_id
traj.joint_names = self.ik_info.kinematic_solver_info.joint_names
#print 'angle_points', len(angle_points[0]), angle_points
for i in range(len(angle_points[0])):
traj.points.append(JointTrajectoryPoint())
traj.points[i].positions = angle_points[:,i]
traj.points[i].velocities = [0]*7
traj.points[i].time_from_start = rospy.Duration(
ang_fracs[i]*duration)
return traj
def hand_frame_move(self, x, y=0, z=0, duration=None):
cp = self.curr_pose()
newpose = pu.pose_relative_trans(cp,x,y,z)
self.dist = pu.calc_dist(cp, newpose)
return newpose