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()