def arm_robot_state(self, robot_state=None, fail_if_joint_not_found=True):
'''
Returns the joint state of the arm (as a robot state) in the current planning scene state or the passed in
state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.
If None, will use the current robot state in the planning scene interface
*fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state
**Returns:**
An arm_navigation_msgs.msg.RobotState containing just the arm joints as they are in robot_state
**Raises:**
**ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
'''
joint_state = self.arm_joint_state(
robot_state=robot_state,
fail_if_joint_not_found=fail_if_joint_not_found)
robot_state = RobotState()
robot_state.joint_state = joint_state
return robot_state
def trajectory_markers(robot_traj, ns='', link_names=None, color=None, a=0.8, scale=1.0, resolution=1):
'''
Returns markers representing the robot trajectory.
The color of each point on the trajectory runs from -60 to 300 in hue with full saturation and value. Note that
-60 is red so this should approximately follow the rainbow.
**Args:**
**robot_traj (arm_navigation_msgs.msg.RobotTrajectory):** Trajectory for which you want markers
*ns (string):* Marker namespace.
*a (double):* Alpha (scale 0 to 1)
*scale (double):* Scaling for entire robot at each point
*resolution (int):* Draws a point only every resolution points on the trajectory. Will always draw
the first and last points.
**Returns:**
visualization_msgs.msg.MarkerArray that will draw the whole trajectory. Each point on the trajectory is
made up of many markers so this will be substantially longer than the length of the trajectory.
'''
if resolution <= 0:
resolution = 1
limit = len(robot_traj.joint_trajectory.points)
if not limit:
limit = len(robot_traj.multi_dof_joint_trajectory.points)
marray = MarkerArray()
for i in range(0, limit, resolution):
robot_state = RobotState()
if len(robot_traj.joint_trajectory.points):
robot_state.joint_state = tt.joint_trajectory_point_to_joint_state(robot_traj.joint_trajectory.points[i],
robot_traj.joint_trajectory.joint_names)
if len(robot_traj.multi_dof_joint_trajectory.points):
robot_state.multi_dof_joint_state = tt.multi_dof_trajectory_point_to_multi_dof_state(
robot_traj.multi_dof_joint_trajectory.points[i], robot_traj.multi_dof_joint_trajectory.joint_names,
robot_traj.multi_dof_joint_trajectory.frame_ids, robot_traj.multi_dof_joint_trajectory.child_frame_ids,
robot_traj.multi_dof_joint_trajectory.stamp)
if not color:
(r, g, b) = hsv_to_rgb((i/float(limit)*360 - 60)%360, 1, 1)
else:
(r, g, b) = (color.r, color.g, color.b)
marray.markers += robot_marker(robot_state, link_names=link_names, ns=ns, r=r, g=g, b=b, a=a, scale=scale).markers
if i != limit-1:
if not color:
(r, g, b) = hsv_to_rgb(300, 1, 1)
else:
(r, g, b) = (color.r, color.g, color.b)
marray.markers += robot_marker(tt.last_state_on_robot_trajectory(robot_traj), link_names=link_names, ns=ns, r=r, g=g,
b=b, a=a, scale=scale).markers
for (i, m) in enumerate(marray.markers):
m.id = i
return marray
def getFK(self,link, service_name='/cob_ik_wrapper/arm/get_fk'):
header=Header()
rs=RobotState()
print "- Try to get JointStates"
rospy.Subscriber("/arm_controller/state",JointTrajectoryControllerState,self.callback)
i=0
while not self.js_received:
i+=1
rospy.sleep(0.1)
if i==10:
i=0
print "[DEBUG] still waiting for message"
joint_states=JointState()
joint_states.header=self.js.header
joint_states.name=self.js.joint_names
joint_states.position=self.js.actual.positions
#print joint_states
print "- wait for service %s" % service_name
rospy.wait_for_service(service_name)
print '- generate header'
header=joint_states.header
header.frame_id='arm_0_link'
#print header
print '- generate fk_link_names'
fk_link_name=['arm_7_link']
#print fk_link_names
print 'generate RobotState message'
rs.joint_state=joint_states
#print rs
arm_fk= rospy.ServiceProxy(service_name, GetPositionFK)
print '*'*10,' Solution ','*'*10
try:
print arm_fk(header,fk_link_name,rs)
return arm_fk(header,fk_link_name,rs)
except e:
print "Service call failed due to error %s" % e
def modify_pickle_file():
[result, goal, collision_objects,
robot_state] = pickle.load(open(sys.argv[1], 'r'))
robot_state = RobotState()
robot_state.multi_dof_joint_state = tt.multi_dof_trajectory_point_to_multi_dof_state(
result.primitive_trajectories[0].multi_dof_joint_trajectory.points[0],
result.primitive_trajectories[0].multi_dof_joint_trajectory.
joint_names,
result.primitive_trajectories[0].multi_dof_joint_trajectory.frame_ids,
result.primitive_trajectories[0].multi_dof_joint_trajectory.
child_frame_ids,
result.primitive_trajectories[0].multi_dof_joint_trajectory.stamp)
robot_state.joint_state = tt.joint_trajectory_point_to_joint_state(
result.primitive_trajectories[0].joint_trajectory.points[0],
result.primitive_trajectories[0].joint_trajectory.joint_names)
pickle.dump([result, goal, collision_objects, robot_state],
open(sys.argv[1], 'wb'))
def last_state_on_robot_trajectory(trajectory):
'''
Returns the last state on a robot trajectory
**Args:**
**trajectory (arm_navigation_msgs.msg.RobotTrajectory):** robot trajectory
**Returns:**
The last state on the trajectory as an arm_navigation_msgs.msg.RobotState
'''
robot_state = RobotState()
robot_state.joint_state = last_state_on_joint_trajectory(trajectory.joint_trajectory)
if not robot_state.joint_state: return None
mdf_traj = trajectory.multi_dof_trajectory
robot_state.multi_dof_joint_state = multi_dof_trajectory_point_to_multi_dof_state\
(mdf_traj.points[-1], mdf_traj.stamp, mdf_traj.joint_names, mdf_traj.frame_ids, mdf_traj.child_frame_ids)
return robot_state
def last_state_on_robot_trajectory(trajectory):
'''
Returns the last state on a robot trajectory
**Args:**
**trajectory (arm_navigation_msgs.msg.RobotTrajectory):** robot trajectory
**Returns:**
The last state on the trajectory as an arm_navigation_msgs.msg.RobotState
'''
robot_state = RobotState()
robot_state.joint_state = last_state_on_joint_trajectory(
trajectory.joint_trajectory)
if not robot_state.joint_state: return None
mdf_traj = trajectory.multi_dof_trajectory
robot_state.multi_dof_joint_state = multi_dof_trajectory_point_to_multi_dof_state\
(mdf_traj.points[-1], mdf_traj.stamp, mdf_traj.joint_names, mdf_traj.frame_ids, mdf_traj.child_frame_ids)
return robot_state
def arm_robot_state(self, robot_state=None, fail_if_joint_not_found=True):
'''
Returns the joint state of the arm (as a robot state) in the current planning scene state or the passed in
state.
**Args:**
*robot_state (arm_navigation_msgs.msg.RobotState):* robot state from which to find the joints.
If None, will use the current robot state in the planning scene interface
*fail_if_joint_not_found (boolean):* Raise a value error if an arm joint is not found in the robot state
**Returns:**
An arm_navigation_msgs.msg.RobotState containing just the arm joints as they are in robot_state
**Raises:**
**ValueError:** if fail_if_joint_not_found is True and an arm joint is not found in the robot state
'''
joint_state = self.arm_joint_state(robot_state=robot_state, fail_if_joint_not_found=fail_if_joint_not_found)
robot_state = RobotState()
robot_state.joint_state = joint_state
return robot_state
def get_trajectory_markers(trajectories,
names,
object_trajectories=None,
link_names=None,
start_id=0):
state = RobotState()
state.joint_state = tt.joint_trajectory_point_to_joint_state(
trajectories[0].joint_trajectory.points[0],
trajectories[0].joint_trajectory.joint_names)
mdf = trajectories[0].multi_dof_joint_trajectory
state.multi_dof_joint_state = tt.multi_dof_trajectory_point_to_multi_dof_state(
mdf.points[0], mdf.joint_names, mdf.frame_ids, mdf.child_frame_ids,
mdf.stamp)
n = names[0].split('-')
c = type_to_color(n[0])
marray = vt.robot_marker(state,
ns='traj_start_state' + str(start_id),
r=c.r,
g=c.g,
b=c.b,
a=0.5)
for (i, m) in enumerate(marray.markers):
m.id = start_id + i
traj = [marray]
mid = start_id
for (i, t) in enumerate(trajectories):
splits = names[i].split('-')
c = type_to_color(splits[0])
rospy.loginfo('Drawing trajectory ' + str(i) + ' of type ' + names[i] +
' and length ' + str(len(t.joint_trajectory.points)))
#print 'Trajectory is\n', t
for j in range(len(t.joint_trajectory.points)):
state.joint_state = tt.joint_trajectory_point_to_joint_state(
t.joint_trajectory.points[j], t.joint_trajectory.joint_names)
state.multi_dof_joint_state = tt.multi_dof_trajectory_point_to_multi_dof_state(
t.multi_dof_joint_trajectory.points[j],
t.multi_dof_joint_trajectory.joint_names,
t.multi_dof_joint_trajectory.frame_ids,
t.multi_dof_joint_trajectory.child_frame_ids,
t.multi_dof_joint_trajectory.stamp)
marray = vt.robot_marker(state,
link_names=link_names,
ns='trajectory',
r=c.r,
g=c.g,
b=c.b,
a=0.5)
#marray = vt.trajectory_markers(t, ns='trajectory', link_names=link_names,resolution=3,
# color=c)
for (i, m) in enumerate(marray.markers):
m.id = mid
mid += 1
traj.append(copy.deepcopy(marray))
return traj
def run_fk(self, angles, link_name):
if link_name not in self.link_names:
rospy.logerr("link name %s not possible!" % link_name)
return None
self.link_name = link_name
header = rospy.Header()
header.stamp = rospy.get_rostime()
header.frame_id = 'base_link'
joint_state = JointState(header, self.joint_names, angles, [], [])
try:
resp = self._fk_service(
header, [link_name],
RobotState(joint_state, MultiDOFJointState()))
except rospy.ServiceException, e:
rospy.logerr("FK service call failed! error msg: %s" % e)
return None
def get_ee_for_state(self, joint_state, multi_dof_joint_state):
self.fk_request.robot_state = RobotState()
self.fk_request.robot_state.joint_state = joint_state
self.fk_request.robot_state.multi_dof_joint_state = multi_dof_joint_state
try:
rospy.loginfo('Requesting FK.')
response = self.fk_srv(self.fk_request)
if(response.error_code.val == response.error_code.SUCCESS):
pose = response.pose_stamped.pose
else:
rospy.logwarn('Could not find FK solution.')
return None
except rospy.ServiceException:
rospy.logerr('Exception while getting the FK solution.')
return None
return pose
def interpolated_ik_motion_planner_callback(self, req):
#names and angles for the joints in their desired order
joint_names = req.motion_plan_request.start_state.joint_state.name
start_angles = req.motion_plan_request.start_state.joint_state.position
#sanity-checking: joint_names and start_angles should be the same length, if any start_angles are specified
if start_angles and len(joint_names) != len(start_angles):
rospy.logerr(
"start_state.joint_state.name needs to be the same length as start_state.joint_state.position! Quitting"
)
return 0
#reorder the start angles to the order needed by IK
reordered_start_angles = []
#get the current joint states for the robot
#joint_states_msg = rospy.wait_for_message('joint_states', JointState, 10.0)
#if not joint_states_msg:
# rospy.logerr("unable to get joint_states message")
# return 0
#get the desired start angles for each IK arm joint in turn from start_state.joint_state.position
#(use the current angle if not specified)
for joint_name in self.ik_utils.joint_names:
#desired start angle specified
if joint_name in joint_names and start_angles:
index = joint_names.index(joint_name)
reordered_start_angles.append(start_angles[index])
else:
rospy.logerr("missing joint angle, can't deal")
return 0
#get additional desired joint angles (such as for the gripper) to pass through to IK
additional_joint_angles = []
additional_joint_names = []
for (ind, joint_name) in enumerate(joint_names):
if joint_name not in self.ik_utils.joint_names:
#rospy.loginfo("found %s"%joint_name)
additional_joint_angles.append(start_angles[ind])
additional_joint_names.append(joint_name)
IK_robot_state = None
if additional_joint_angles:
#rospy.loginfo("adding additional start angles for:"+str(additional_joint_names))
#rospy.loginfo("additional joint angles:"+str(additional_joint_angles))
IK_robot_state = RobotState()
IK_robot_state.joint_state.name = additional_joint_names
IK_robot_state.joint_state.position = additional_joint_angles
start_pose_stamped = self.ik_utils.run_fk(reordered_start_angles,
self.ik_utils.link_name)
#the desired goal position
goal_pos = req.motion_plan_request.goal_constraints.position_constraints[
0].position
#the frame that goal position is in
goal_pos_frame = req.motion_plan_request.goal_constraints.position_constraints[
0].header.frame_id
#convert the position to base_link frame
goal_ps = self.add_header(PointStamped(), goal_pos_frame)
goal_ps.point = goal_pos
goal_pos_list = self.ik_utils.point_stamped_to_list(
goal_ps, 'base_link')
#the desired goal orientation
goal_quat = req.motion_plan_request.goal_constraints.orientation_constraints[
0].orientation
#the frame that goal orientation is in
goal_quat_frame = req.motion_plan_request.goal_constraints.orientation_constraints[
0].header.frame_id
#convert the quaternion to base_link frame
goal_qs = self.add_header(QuaternionStamped(), goal_quat_frame)
goal_qs.quaternion = goal_quat
goal_quat_list = self.ik_utils.quaternion_stamped_to_list(
goal_qs, 'base_link')
#assemble the goal pose into a PoseStamped
goal_pose_stamped = self.add_header(PoseStamped(), 'base_link')
goal_pose_stamped.pose = Pose(Point(*goal_pos_list),
Quaternion(*goal_quat_list))
#get the ordered collision operations, if there are any
ordered_collision_operations = None #req.motion_plan_request.ordered_collision_operations
#if ordered_collision_operations.collision_operations == []:
# ordered_collision_operations = None
#get the link paddings, if there are any
link_padding = None #req.motion_plan_request.link_padding
#if link_padding == []:
# link_padding = None
#RUN! Check the Cartesian path for consistent, non-colliding IK solutions
(trajectory, error_codes) = self.ik_utils.check_cartesian_path(start_pose_stamped, \
goal_pose_stamped, reordered_start_angles, self.pos_spacing, self.rot_spacing, \
self.consistent_angle, self.collision_aware, self.collision_check_resolution, \
self.steps_before_abort, self.num_steps, ordered_collision_operations, \
self.start_from_end, IK_robot_state, link_padding)
#find appropriate velocities and times for the valid part of the resulting joint path (invalid parts set to 0)
#if we're searching from the end, keep the end; if we're searching from the start, keep the start
start_ind = 0
stop_ind = len(error_codes)
if self.start_from_end:
for ind in range(len(error_codes) - 1, 0, -1):
if error_codes[ind]:
start_ind = ind + 1
break
else:
for ind in range(len(error_codes)):
if error_codes[ind]:
stop_ind = ind
break
(times, vels) = self.ik_utils.trajectory_times_and_vels(
trajectory[start_ind:stop_ind], self.max_joint_vels,
self.max_joint_accs)
times = [0] * start_ind + times + [0] * (len(error_codes) - stop_ind)
vels = [[0] * 7] * start_ind + vels + [[0] * 7] * (len(error_codes) -
stop_ind)
rospy.logdebug("trajectory:")
for ind in range(len(trajectory)):
rospy.logdebug("error code " + str(error_codes[ind]) + " pos : " +
self.pplist(trajectory[ind]))
rospy.logdebug("")
for ind in range(len(trajectory)):
rospy.logdebug("time: " + "%5.3f " % times[ind] + "vels: " +
self.pplist(vels[ind]))
#the response
res = GetMotionPlanResponse()
#the arm joint names in the normal order, as spit out by IKQuery
res.trajectory.joint_trajectory.joint_names = self.ik_utils.joint_names[:]
#a list of 7-lists of joint angles, velocities, and times for a trajectory that gets you from start to goal
#(all 0s if there was no IK solution for a point on the path)
res.trajectory.joint_trajectory.points = []
for i in range(len(trajectory)):
joint_trajectory_point = JointTrajectoryPoint()
joint_trajectory_point.positions = trajectory[i]
joint_trajectory_point.velocities = vels[i]
joint_trajectory_point.time_from_start = rospy.Duration(times[i])
res.trajectory.joint_trajectory.points.append(
joint_trajectory_point)
#a list of ArmNavigationErrorCodes messages, one for each trajectory point, with values as follows:
#ArmNavigationErrorCodes.SUCCESS (1): no problem
#ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED (-23): no non-colliding IK solution (colliding solution provided)
#ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED (-20): inconsistency in path between this point and the next point
#ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED (-21): out of reach (no colliding solution)
#ArmNavigationErrorCodes.PLANNING_FAILED (0): aborted before getting to this point
error_code_dict = {0:ArmNavigationErrorCodes.SUCCESS, 1:ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED, \
2:ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED, 3:ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED, \
4:ArmNavigationErrorCodes.PLANNING_FAILED}
trajectory_error_codes = [
ArmNavigationErrorCodes(val=error_code_dict[error_code])
for error_code in error_codes
]
res.trajectory_error_codes = trajectory_error_codes
res.error_code.val = ArmNavigationErrorCodes.SUCCESS if max(
error_codes) == 0 else ArmNavigationErrorCodes.PLANNING_FAILED
# rospy.loginfo("trajectory:")
# for ind in range(len(trajectory)):
# rospy.loginfo("error code "+ str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
# rospy.loginfo("")
# for ind in range(len(trajectory)):
# rospy.loginfo("time: " + "%5.3f "%times[ind] + "vels: " + self.pplist(vels[ind]))
return res
def plan_interpolated_ik(self,
pose_stamped,
starting_pose=None,
ordered_collisions=None,
bounds=None,
starting_state=None,
min_acceptable_distance=None,
collision_aware=True,
reverse=False,
resolution=0.005,
nsteps=0,
consistent_angle=np.pi / 6.0,
steps_before_abort=0,
collision_check_resolution=2,
max_joint_vels=None,
max_joint_accs=None):
'''
Plans a path that follows a straight line in Cartesian space.
This function is useful for tasks like grasping where it is necessary to ensure that the gripper moves in a
straight line relative to the world. The trajectory returned from this function is safe to execute.
This function may alter the planning scene during planning, but returns it to its initial state when
finished.
There are a lot of possible arguments to this function but in general the defaults work well.
**Args:**
**pose_stamped (geomety_msgs.msg.PoseStamped):** The ending pose for the hand frame defined in hand (see
hand_description.py)
*starting_pose (geometry_msgs.msg.PoseStamped):* The starting pose of the hand frame. If None, this will
use the current pose of the hand frame in the starting state
*ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):* Any additional collision
operations besides those in the planning scene interface you want to use during planning.
*bounds ([double]):* Acceptable errors for the goal position as (x, y, z, roll, pitch, yaw). If nothing
is passed in, uses the defaults defined in conversions.py
*starting_state (arm_navigation_msgs.msg.RobotState):* The state of the robot at the start of the plan if
reverse is False and at the end of the plan if reverse is True. If you pass in a starting_pose that
does not match the starting state, the planner will use the starting_pose not the stating_state. If you
pass in a starting state and no starting pose, the planner will use the hand frame pose in the starting
state. If you pass in no starting state, but you do pass in a starting_pose, the planner will solve for
a collision free IK solution for the starting state. If you pass in no starting state or starting pose,
the planner will use the current robot state in the planning scene interface. If reverse is False, the
starting state will be appended to the front of the trajectory.
*min_acceptable_distance (double):* If the planner finds a path of at least this distance (in meters),
it is a success. This must be greater than zero; to disable, set to None.
*collision_aware (boolean):* Set to False if you want no collision checking to be done
*reverse (boolean):* Set to True if you want the planner to start planning at pose_stamped and try to
plan towards starting_pose. The trajectory returned will still end at pose_stamped.
*resolution (double):* The resolution in centimeters between points on the trajectory in Carteisan space.
Will only be used if nsteps=0.
*nsteps (int):* The number of steps you want on the trajectory. If nsteps is set, resolution will be
ignored.
*consistent_angle (double):* If any joint angle between two points on this trajectory changes by more
than this amount, the planning will fail.
*steps_before_abort (int):* The number of invalid steps (no IK solution etc) allowed before the planning
fails.
*collision_check_resolution (int):* Collisions will be checked every collision_check_resolution steps
*max_joint_vels ([double]):* Maximum allowed joint velocities. If not passed in, will be set to 0.1 for
all joints.
*max_joint_accs ([double]):* Maximum allowed joint accelerations. If not passed in, only velocity
constraints will be used.
**Returns:**
A trajectory_msgs.msg.JointTrajectory that is safe to execute in which the hand frame moves in a straight
line in Cartesian space.
**Raises:**
**exceptions.ArmNavError:** if no plan can be found.
**rospy.ServiceException:** if there is a problem with the call to the planning or parameter service
'''
#prior_state = self._psi.get_robot_state()
self._psi.add_ordered_collisions(ordered_collisions)
#if starting_state:
#starting_state = self.get_closest_state_in_limits(robot_state=starting_state)
#self._psi.set_robot_state(starting_state)
if not starting_pose:
starting_pose = self.get_hand_frame_pose(
robot_state=starting_state,
frame_id=pose_stamped.header.frame_id)
else:
starting_pose = self._psi.transform_pose_stamped(
pose_stamped.header.frame_id, starting_pose)
if starting_state:
#check that it matches
if not reverse:
chk_pose = starting_pose
else:
chk_pose = pose_stamped
starting_fk = self.get_hand_frame_pose(
robot_state=starting_state, frame_id=chk_pose.header.frame_id)
if not gt.near(starting_fk.pose, chk_pose.pose):
rospy.logwarn(
'Input starting state does not match starting pose. ' +
'Solving for an IK solution instead')
rospy.logdebug('Starting FK is\n' + str(starting_fk) +
'\nCheck pose is\n' + str(chk_pose))
rospy.logdebug(
'Euclidean distance is: ' + str(
gt.euclidean_distance(starting_fk.pose.position,
chk_pose.pose.position)) +
', angular distance is: ' + str(
gt.quaternion_distance(starting_fk.pose.orientation,
chk_pose.pose.orientation)))
starting_state = None
if not starting_state:
if reverse:
ik_sol = self.get_ik(pose_stamped,
collision_aware=collision_aware)
else:
ik_sol = self.get_ik(starting_pose,
collision_aware=collision_aware)
if ik_sol.error_code.val != ik_sol.error_code.SUCCESS:
rospy.logerr(
'Starting pose for interpolated IK had IK error ' +
str(ik_sol.error_code.val))
raise ArmNavError(
'Starting pose for interpolated IK had no IK solution',
error_code=ik_sol.error_code)
starting_state = ik_sol.solution
#self._psi.set_robot_state(starting_state)
rospy.logdebug('Planning interpolated IK from\n' + str(starting_pose) +
'\nto\n' + str(pose_stamped))
init_state = RobotState()
init_state.joint_state = starting_state.joint_state
init_state.multi_dof_joint_state.frame_ids.append(
starting_pose.header.frame_id)
init_state.multi_dof_joint_state.child_frame_ids.append(
self.hand.hand_frame)
init_state.multi_dof_joint_state.poses.append(starting_pose.pose)
goal = conv.pose_stamped_to_motion_plan_request(pose_stamped,
self.hand.hand_frame,
self.arm_name,
init_state,
bounds=bounds)
dist = gt.euclidean_distance(pose_stamped.pose.position,
starting_pose.pose.position)
if nsteps == 0:
if resolution == 0:
rospy.logwarn(
'Resolution and steps were both zero in interpolated IK. '
+ 'Using default resolution of 0.005')
resolution = 0.005
nsteps = int(dist / resolution)
res = dist / nsteps
req = SetInterpolatedIKMotionPlanParamsRequest()
req.num_steps = nsteps
req.consistent_angle = consistent_angle
req.collision_check_resolution = collision_check_resolution
req.steps_before_abort = steps_before_abort
req.collision_aware = collision_aware
req.start_from_end = reverse
if max_joint_vels:
req.max_joint_vels = max_joint_vels
if max_joint_accs:
req.max_joint_accs = max_joint_accs
self._interpolated_ik_parameter_service(req)
rospy.loginfo(
'Calling interpolated ik motion planning service. Expecting ' +
str(nsteps) + ' steps')
rospy.logdebug('Sending goal\n' + str(goal))
ik_resp = self._interpolated_ik_planning_service(goal)
self._psi.remove_ordered_collisions(ordered_collisions)
#self._psi.set_robot_state(prior_state)
traj = ik_resp.trajectory.joint_trajectory
first_index = 0
rospy.logdebug('Trajectory error codes are ' +
str([e.val for e in ik_resp.trajectory_error_codes]))
if reverse:
for first_index in range(len(ik_resp.trajectory_error_codes)):
e = ik_resp.trajectory_error_codes[first_index]
if e.val == e.SUCCESS:
break
last_index = 0
e = ArmNavigationErrorCodes()
e.val = e.SUCCESS
for last_index in range(first_index,
len(ik_resp.trajectory_error_codes) + 1):
if last_index == len(ik_resp.trajectory_error_codes):
#the whole trajectory works
break
e = ik_resp.trajectory_error_codes[last_index]
if e.val != e.SUCCESS:
rospy.logerr('Interpolated IK failed with error ' +
str(e.val) + ' on step ' + str(last_index) +
' after distance ' +
str((last_index + 1 - first_index) * res))
last_index -= 1
break
rospy.logdebug('First index = ' + str(first_index) +
', last index = ' + str(last_index))
distance = (last_index - first_index) * res
traj.points = traj.points[first_index:max(0, last_index)]
rospy.loginfo('Interpolated IK returned trajectory with ' +
str(len(traj.points)) + ' points')
if e.val != e.SUCCESS and (not min_acceptable_distance
or distance < min_acceptable_distance):
raise ArmNavError(
'Interpolated IK failed after ' +
str(last_index - first_index) + ' steps.',
error_code=e,
trajectory_error_codes=ik_resp.trajectory_error_codes,
trajectory=traj)
if not reverse or not traj.points:
return tt.add_state_to_front_of_joint_trajectory(
self.arm_joint_state(robot_state=starting_state), traj)
return traj
def plan_interpolated_ik(self, pose_stamped, starting_pose=None, ordered_collisions=None, bounds = None,
starting_state=None, min_acceptable_distance=None, collision_aware=True,
reverse=False, resolution=0.005, nsteps=0, consistent_angle=np.pi/6.0,
steps_before_abort=0, collision_check_resolution=2, max_joint_vels=None,
max_joint_accs=None):
'''
Plans a path that follows a straight line in Cartesian space.
This function is useful for tasks like grasping where it is necessary to ensure that the gripper moves in a
straight line relative to the world. The trajectory returned from this function is safe to execute.
This function may alter the planning scene during planning, but returns it to its initial state when
finished.
There are a lot of possible arguments to this function but in general the defaults work well.
**Args:**
**pose_stamped (geomety_msgs.msg.PoseStamped):** The ending pose for the hand frame defined in hand (see
hand_description.py)
*starting_pose (geometry_msgs.msg.PoseStamped):* The starting pose of the hand frame. If None, this will
use the current pose of the hand frame in the starting state
*ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):* Any additional collision
operations besides those in the planning scene interface you want to use during planning.
*bounds ([double]):* Acceptable errors for the goal position as (x, y, z, roll, pitch, yaw). If nothing
is passed in, uses the defaults defined in conversions.py
*starting_state (arm_navigation_msgs.msg.RobotState):* The state of the robot at the start of the plan if
reverse is False and at the end of the plan if reverse is True. If you pass in a starting_pose that
does not match the starting state, the planner will use the starting_pose not the stating_state. If you
pass in a starting state and no starting pose, the planner will use the hand frame pose in the starting
state. If you pass in no starting state, but you do pass in a starting_pose, the planner will solve for
a collision free IK solution for the starting state. If you pass in no starting state or starting pose,
the planner will use the current robot state in the planning scene interface. If reverse is False, the
starting state will be appended to the front of the trajectory.
*min_acceptable_distance (double):* If the planner finds a path of at least this distance (in meters),
it is a success. This must be greater than zero; to disable, set to None.
*collision_aware (boolean):* Set to False if you want no collision checking to be done
*reverse (boolean):* Set to True if you want the planner to start planning at pose_stamped and try to
plan towards starting_pose. The trajectory returned will still end at pose_stamped.
*resolution (double):* The resolution in centimeters between points on the trajectory in Carteisan space.
Will only be used if nsteps=0.
*nsteps (int):* The number of steps you want on the trajectory. If nsteps is set, resolution will be
ignored.
*consistent_angle (double):* If any joint angle between two points on this trajectory changes by more
than this amount, the planning will fail.
*steps_before_abort (int):* The number of invalid steps (no IK solution etc) allowed before the planning
fails.
*collision_check_resolution (int):* Collisions will be checked every collision_check_resolution steps
*max_joint_vels ([double]):* Maximum allowed joint velocities. If not passed in, will be set to 0.1 for
all joints.
*max_joint_accs ([double]):* Maximum allowed joint accelerations. If not passed in, only velocity
constraints will be used.
**Returns:**
A trajectory_msgs.msg.JointTrajectory that is safe to execute in which the hand frame moves in a straight
line in Cartesian space.
**Raises:**
**exceptions.ArmNavError:** if no plan can be found.
**rospy.ServiceException:** if there is a problem with the call to the planning or parameter service
'''
#prior_state = self._psi.get_robot_state()
self._psi.add_ordered_collisions(ordered_collisions)
#if starting_state:
#starting_state = self.get_closest_state_in_limits(robot_state=starting_state)
#self._psi.set_robot_state(starting_state)
if not starting_pose:
starting_pose = self.get_hand_frame_pose(robot_state=starting_state, frame_id=pose_stamped.header.frame_id)
else:
starting_pose = self._psi.transform_pose_stamped(pose_stamped.header.frame_id, starting_pose)
if starting_state:
#check that it matches
if not reverse:
chk_pose = starting_pose
else:
chk_pose = pose_stamped
starting_fk = self.get_hand_frame_pose(robot_state=starting_state,
frame_id=chk_pose.header.frame_id)
if not gt.near(starting_fk.pose, chk_pose.pose):
rospy.logwarn('Input starting state does not match starting pose. '+
'Solving for an IK solution instead')
rospy.logdebug('Starting FK is\n'+str(starting_fk)+'\nCheck pose is\n'+str(chk_pose))
rospy.logdebug('Euclidean distance is: '+
str(gt.euclidean_distance(starting_fk.pose.position, chk_pose.pose.position))+
', angular distance is: '+
str(gt.quaternion_distance(starting_fk.pose.orientation, chk_pose.pose.orientation)))
starting_state = None
if not starting_state:
if reverse:
ik_sol = self.get_ik(pose_stamped, collision_aware=collision_aware)
else:
ik_sol = self.get_ik(starting_pose, collision_aware=collision_aware)
if ik_sol.error_code.val != ik_sol.error_code.SUCCESS:
rospy.logerr('Starting pose for interpolated IK had IK error '+str(ik_sol.error_code.val))
raise ArmNavError('Starting pose for interpolated IK had no IK solution',
error_code = ik_sol.error_code)
starting_state = ik_sol.solution
#self._psi.set_robot_state(starting_state)
rospy.logdebug('Planning interpolated IK from\n'+str(starting_pose)+'\nto\n'+str(pose_stamped))
init_state = RobotState()
init_state.joint_state = starting_state.joint_state
init_state.multi_dof_joint_state.frame_ids.append(starting_pose.header.frame_id)
init_state.multi_dof_joint_state.child_frame_ids.append(self.hand.hand_frame)
init_state.multi_dof_joint_state.poses.append(starting_pose.pose)
goal = conv.pose_stamped_to_motion_plan_request(pose_stamped, self.hand.hand_frame, self.arm_name,
init_state, bounds=bounds)
dist = gt.euclidean_distance(pose_stamped.pose.position, starting_pose.pose.position)
if nsteps == 0:
if resolution == 0:
rospy.logwarn('Resolution and steps were both zero in interpolated IK. '+
'Using default resolution of 0.005')
resolution = 0.005
nsteps = int(dist/resolution)
res = dist/nsteps
req = SetInterpolatedIKMotionPlanParamsRequest()
req.num_steps = nsteps
req.consistent_angle = consistent_angle
req.collision_check_resolution = collision_check_resolution
req.steps_before_abort = steps_before_abort
req.collision_aware = collision_aware
req.start_from_end = reverse
if max_joint_vels:
req.max_joint_vels = max_joint_vels
if max_joint_accs:
req.max_joint_accs = max_joint_accs
self._interpolated_ik_parameter_service(req)
rospy.loginfo('Calling interpolated ik motion planning service. Expecting '+str(nsteps)+' steps')
rospy.logdebug('Sending goal\n'+str(goal))
ik_resp = self._interpolated_ik_planning_service(goal)
self._psi.remove_ordered_collisions(ordered_collisions)
#self._psi.set_robot_state(prior_state)
traj = ik_resp.trajectory.joint_trajectory
first_index = 0
rospy.logdebug('Trajectory error codes are '+str([e.val for e in ik_resp.trajectory_error_codes]))
if reverse:
for first_index in range(len(ik_resp.trajectory_error_codes)):
e = ik_resp.trajectory_error_codes[first_index]
if e.val == e.SUCCESS:
break
last_index = 0
e = ArmNavigationErrorCodes()
e.val = e.SUCCESS
for last_index in range(first_index,len(ik_resp.trajectory_error_codes)+1):
if last_index == len(ik_resp.trajectory_error_codes):
#the whole trajectory works
break
e = ik_resp.trajectory_error_codes[last_index]
if e.val != e.SUCCESS:
rospy.logerr('Interpolated IK failed with error '+str(e.val)+' on step ' +str(last_index)+
' after distance '+ str((last_index+1-first_index)*res))
last_index -= 1
break
rospy.logdebug('First index = '+str(first_index)+', last index = '+str(last_index))
distance = (last_index-first_index)*res
traj.points = traj.points[first_index:max(0,last_index)]
rospy.loginfo('Interpolated IK returned trajectory with '+str(len(traj.points))+' points')
if e.val != e.SUCCESS and (not min_acceptable_distance or distance < min_acceptable_distance):
raise ArmNavError('Interpolated IK failed after '+str(last_index-first_index)+' steps.', error_code=e,
trajectory_error_codes = ik_resp.trajectory_error_codes, trajectory=traj)
if not reverse or not traj.points:
return tt.add_state_to_front_of_joint_trajectory(self.arm_joint_state(robot_state=starting_state), traj)
return traj
