def get_ik(self, pose_stamped, collision_aware=True, starting_state=None,
seed_state=None, timeout=5.0, ordered_collisions=None):
'''
Solves inverse kinematics problems.
**Args:**
**pose_stamped (geometry_msgs.msg.PoseStamped):** The pose for which to get joint positions
*collision_aware (boolean):* If True, returns a solution that does not collide with anything in the world
*starting_state (arm_navigation_msgs.msg.RobotState):* The returned state will have all non-arm joints
matching this state. If no state is passed in, it will use the current state in the planning scene
interface.
*seed_state (arm_navigation_msgs.msg.RobotState):* A seed state for IK. If no state is passed it, it
will use the current state in planning scene interface.
*timeout (double):* Time in seconds that IK is allowed to run
**Returns:**
A kinematics_msgs.srv.GetConstraintAwarePositionIKResponse if collision_aware was True and a
kinematics_msgs.srv.GetPosiitonIKResponse if collision_aware was False. The robot state returned has
the arm joints set to the IK solution if found and all other joints set to that of starting_state.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls an IK service which may use TF for transformations! Probably best to only use with pose stampeds
defined in the robot frame (convert them yourself using the planning scene interface).
'''
rospy.logdebug('Solving IK for\n'+str(pose_stamped))
pos_req = PositionIKRequest()
pos_req.ik_link_name = self.hand.hand_frame
pos_req.pose_stamped = pose_stamped
if not starting_state:
starting_state = self._psi.get_robot_state()
if not seed_state:
seed_state = self.arm_robot_state(starting_state)
pos_req.ik_seed_state = seed_state
pos_req.robot_state = starting_state
if collision_aware:
self._psi.add_ordered_collisions(ordered_collisions)
coll_req = GetConstraintAwarePositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._collision_aware_ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(coll_res.solution.joint_state,
copy.deepcopy(starting_state))
self._psi.remove_ordered_collisions(ordered_collisions)
return coll_res
coll_req = GetPositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(coll_res.solution.joint_state,
copy.deepcopy(starting_state))
return coll_res
def plan_joints_collision_free(self, joint_state, starting_state=None, ordered_collisions=None, timeout=15.0,
bounds=None, planner_id='', ntries=3):
'''
**Deprecated.**
Use plan_collision_free.
'''
starting_state = self.get_closest_state_in_limits(robot_state=starting_state)
goal = conv.joint_state_to_motion_plan_request(joint_state, self.hand.hand_frame, self.arm_name,
starting_state, timeout=timeout,
bounds=bounds, planner_id=planner_id)
goal_state = tt.set_joint_state_in_robot_state(joint_state, copy.deepcopy(starting_state))
rospy.loginfo('Position of wrist is\n'+str(self._transformer.get_transform
('l_wrist_roll_link', self._psi.robot_frame, goal_state)))
rospy.loginfo('Position of fingertip is\n'+str(self._transformer.get_transform('l_gripper_r_finger_tip_link',
self._psi.robot_frame,
goal_state)))
rospy.loginfo('Collision objects are\n')
cos = self._psi.collision_objects()
for co in cos:
if 'table' not in co.id:
rospy.loginfo(str(co))
rospy.loginfo('Attached collision objects are\n')
aos = self._psi.attached_collision_objects()
for ao in aos:
rospy.loginfo(ao)
return self.plan_collision_free(goal, ordered_collisions=ordered_collisions, ntries=ntries)
def _move_out_of_collision(self, move_mag=0.3, num_tries=100):
'''
Tries to find a small movement that will take the arm out of collision.
**Args:**
*move_mag (float):* Max magnitude in radians of movement for each joint.
*num_tries (int):* Number of random joint angles to try before giving up.
**Returns:**
succeeded (boolean): True if arm was sucessfully moved out of collision.
'''
req = GetStateValidityRequest()
req.robot_state = self.world_interface.get_robot_state()
req.check_collisions = True
req.check_path_constraints = False
req.check_joint_limits = False
req.group_name = self._arm_name
res = self._planner.get_state_validity_service(req)
if res.error_code.val == ArmNavErrorCodes.SUCCESS:
rospy.logdebug('Current state not in collision')
return False
joint_state = self._planner.arm_joint_state()
current_joint_position = np.array(joint_state.position)
for ii in range(num_tries):
joint_position = current_joint_position + np.random.uniform(
-move_mag, move_mag, (len(joint_state.position), ))
joint_state.position = list(joint_position)
trajectory_tools.set_joint_state_in_robot_state(
joint_state, req.robot_state)
res = self._planner.get_state_validity_service(req)
in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS)
rospy.logdebug('%s in collision: %s' %
(str(joint_position), str(in_collision)))
if not in_collision:
self._move_to_goal_directly(joint_state,
None,
None,
collision_aware=False)
self._current_handle._set_reached_goal(True)
self._current_handle._set_reached_goal(False)
def is_in_collision(self, arm_name, joint_state, check_joint_limits=False):
'''
Tells you whether the current arm pose is in collision.
**Returns:**
succeeded (boolean): True if arm is in collision.
'''
req = GetStateValidityRequest()
req.robot_state = self._world_interface.get_robot_state()
trajectory_tools.set_joint_state_in_robot_state(joint_state, req.robot_state)
req.check_collisions = True
req.check_path_constraints = False
req.check_joint_limits = check_joint_limits
req.group_name = self._arm_name
res = self._planners[arm_name].get_state_validity_service(req)
if res.error_code.val == ArmNavErrorCodes.SUCCESS:
rospy.logdebug('Current state not in collision')
return False
else:
rospy.logdebug('Current state in collision')
return True
def is_in_collision(self, arm_name, joint_state, check_joint_limits=False):
'''
Tells you whether the current arm pose is in collision.
**Returns:**
succeeded (boolean): True if arm is in collision.
'''
req = GetStateValidityRequest()
req.robot_state = self._world_interface.get_robot_state()
trajectory_tools.set_joint_state_in_robot_state(
joint_state, req.robot_state)
req.check_collisions = True
req.check_path_constraints = False
req.check_joint_limits = check_joint_limits
req.group_name = self._arm_name
res = self._planners[arm_name].get_state_validity_service(req)
if res.error_code.val == ArmNavErrorCodes.SUCCESS:
rospy.logdebug('Current state not in collision')
return False
else:
rospy.logdebug('Current state in collision')
return True
def _move_out_of_collision(self, move_mag=0.3, num_tries=100):
'''
Tries to find a small movement that will take the arm out of collision.
**Args:**
*move_mag (float):* Max magnitude in radians of movement for each joint.
*num_tries (int):* Number of random joint angles to try before giving up.
**Returns:**
succeeded (boolean): True if arm was sucessfully moved out of collision.
'''
req = GetStateValidityRequest()
req.robot_state = self.world_interface.get_robot_state()
req.check_collisions = True
req.check_path_constraints = False
req.check_joint_limits = False
req.group_name = self._arm_name
res = self._planner.get_state_validity_service(req)
if res.error_code.val == ArmNavErrorCodes.SUCCESS:
rospy.logdebug('Current state not in collision')
return False
joint_state = self._planner.arm_joint_state()
current_joint_position = np.array(joint_state.position)
for ii in range(num_tries):
joint_position = current_joint_position + np.random.uniform(
-move_mag, move_mag, (len(joint_state.position),))
joint_state.position = list(joint_position)
trajectory_tools.set_joint_state_in_robot_state(joint_state, req.robot_state)
res = self._planner.get_state_validity_service(req)
in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS)
rospy.logdebug('%s in collision: %s' % (str(joint_position), str(in_collision)))
if not in_collision:
self._move_to_goal_directly(joint_state, None, None, collision_aware=False)
self._current_handle._set_reached_goal(True)
self._current_handle._set_reached_goal(False)
def plan_arm(self, goal, ordered_colls=None, interpolated=False):
time = 0
robot_state = self.psi.get_robot_state()
ending_robot_state = copy.deepcopy(robot_state)
start_markers = vt.robot_marker(robot_state, ns='arm_starting_state')
self.publish(start_markers)
marray = MarkerArray()
marker = vt.marker_at(goal, ns="arm_goal_pose")
marray.markers.append(marker)
self.publish(marray)
if not interpolated:
traj, time = self.arm_planner.plan_collision_free(
goal, ordered_collisions=ordered_colls, runtime=True)
else:
goal_bl = self.psi.transform_pose_stamped('base_link', goal)
marray = MarkerArray()
marker = vt.marker_at(goal_bl, ns="arm_goal_pose_base_link")
marray.markers.append(marker)
self.publish(marray)
traj, time = self.arm_planner.plan_interpolated_ik(
goal_bl,
ordered_collisions=ordered_colls,
consistent_angle=2.0 * np.pi,
runtime=True)
joint_state = tt.joint_trajectory_point_to_joint_state(
traj.points[-1], traj.joint_names)
ending_robot_state = tt.set_joint_state_in_robot_state(
joint_state, ending_robot_state)
goal_markers = vt.robot_marker(ending_robot_state,
ns='arm_goal_state',
g=1,
b=0)
self.publish(goal_markers)
#do the trajectory
# traj.points = [traj.points[0], traj.points[1], traj.points[-1]]
# traj.header.stamp = rospy.Time.now()
# traj.points[-1].time_from_start = 3
#rospy.loginfo('Trajectory =\n'+str(traj))
self.arm_mover.execute_joint_trajectory('right_arm', traj)
#self.psi.set_robot_state(ending_robot_state)
return time
def set_joint_positions_in_robot_state(self, joint_pos, robot_state):
'''
Sets joint positions in a robot state.
**Args:**
**joint_pos ([double]):** An array of joint positions
**robot_state (arm_navigation_msgs.msg.RobotState):** A robot state in which to set the joint angles
equal to joint_pos
**Returns:**
An arm_navigation_msgs.msg.RobotState in which the joint angles corresponding to this arm are set to
joint_pos (assumes these are in the same order as joint_names) and all other joints match the passed
in robot_state. Also sets these joints in the passed in robot state.
'''
return tt.set_joint_state_in_robot_state(self.joint_positions_to_joint_state(joint_pos), robot_state)
def set_joint_positions_in_robot_state(self, joint_pos, robot_state):
'''
Sets joint positions in a robot state.
**Args:**
**joint_pos ([double]):** An array of joint positions
**robot_state (arm_navigation_msgs.msg.RobotState):** A robot state in which to set the joint angles
equal to joint_pos
**Returns:**
An arm_navigation_msgs.msg.RobotState in which the joint angles corresponding to this arm are set to
joint_pos (assumes these are in the same order as joint_names) and all other joints match the passed
in robot_state. Also sets these joints in the passed in robot state.
'''
return tt.set_joint_state_in_robot_state(
self.joint_positions_to_joint_state(joint_pos), robot_state)
def plan_joints_collision_free(self,
joint_state,
starting_state=None,
ordered_collisions=None,
timeout=15.0,
bounds=None,
planner_id='',
ntries=3):
'''
**Deprecated.**
Use plan_collision_free.
'''
starting_state = self.get_closest_state_in_limits(
robot_state=starting_state)
goal = conv.joint_state_to_motion_plan_request(joint_state,
self.hand.hand_frame,
self.arm_name,
starting_state,
timeout=timeout,
bounds=bounds,
planner_id=planner_id)
goal_state = tt.set_joint_state_in_robot_state(
joint_state, copy.deepcopy(starting_state))
rospy.loginfo('Position of wrist is\n' + str(
self._transformer.get_transform(
'l_wrist_roll_link', self._psi.robot_frame, goal_state)))
rospy.loginfo('Position of fingertip is\n' + str(
self._transformer.get_transform('l_gripper_r_finger_tip_link', self
._psi.robot_frame, goal_state)))
rospy.loginfo('Collision objects are\n')
cos = self._psi.collision_objects()
for co in cos:
if 'table' not in co.id:
rospy.loginfo(str(co))
rospy.loginfo('Attached collision objects are\n')
aos = self._psi.attached_collision_objects()
for ao in aos:
rospy.loginfo(ao)
return self.plan_collision_free(goal,
ordered_collisions=ordered_collisions,
ntries=ntries)
def get_ik(self,
pose_stamped,
collision_aware=True,
starting_state=None,
seed_state=None,
timeout=5.0,
ordered_collisions=None):
'''
Solves inverse kinematics problems.
**Args:**
**pose_stamped (geometry_msgs.msg.PoseStamped):** The pose for which to get joint positions
*collision_aware (boolean):* If True, returns a solution that does not collide with anything in the world
*starting_state (arm_navigation_msgs.msg.RobotState):* The returned state will have all non-arm joints
matching this state. If no state is passed in, it will use the current state in the planning scene
interface.
*seed_state (arm_navigation_msgs.msg.RobotState):* A seed state for IK. If no state is passed it, it
will use the current state in planning scene interface.
*timeout (double):* Time in seconds that IK is allowed to run
**Returns:**
A kinematics_msgs.srv.GetConstraintAwarePositionIKResponse if collision_aware was True and a
kinematics_msgs.srv.GetPosiitonIKResponse if collision_aware was False. The robot state returned has
the arm joints set to the IK solution if found and all other joints set to that of starting_state.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls an IK service which may use TF for transformations! Probably best to only use with pose stampeds
defined in the robot frame (convert them yourself using the planning scene interface).
'''
rospy.logdebug('Solving IK for\n' + str(pose_stamped))
pos_req = PositionIKRequest()
pos_req.ik_link_name = self.hand.hand_frame
pos_req.pose_stamped = pose_stamped
if not starting_state:
starting_state = self._psi.get_robot_state()
if not seed_state:
seed_state = self.arm_robot_state(starting_state)
pos_req.ik_seed_state = seed_state
pos_req.robot_state = starting_state
if collision_aware:
self._psi.add_ordered_collisions(ordered_collisions)
coll_req = GetConstraintAwarePositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._collision_aware_ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(
coll_res.solution.joint_state, copy.deepcopy(starting_state))
self._psi.remove_ordered_collisions(ordered_collisions)
return coll_res
coll_req = GetPositionIKRequest()
coll_req.ik_request = pos_req
coll_req.timeout = rospy.Duration(timeout)
coll_res = self._ik_service(coll_req)
coll_res.solution = tt.set_joint_state_in_robot_state(
coll_res.solution.joint_state, copy.deepcopy(starting_state))
return coll_res
def set_planning_scene_joint_state(js):
psi = get_planning_scene_interface()
rs = psi.get_robot_state()
set_joint_state_in_robot_state(js, rs)
psi.set_robot_state(rs)
psi.send_diff()
def set_planning_scene_joint_state(js):
psi = get_planning_scene_interface()
rs = psi.get_robot_state()
set_joint_state_in_robot_state(js,rs)
psi.set_robot_state(rs)
psi.send_diff()
