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