Пример #1
0
 def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor):
     ser_ref_state_in = conversions.msg_to_string(ref_state_in)
     ser_traj_in = conversions.msg_to_string(traj_in)
     ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor)
     traj_out = RobotTrajectory()
     traj_out.deserialize(ser_traj_out)
     return traj_out
Пример #2
0
 def pick(self, object_name, grasp=[]):
     """Pick the named object. A grasp message, or a list of Grasp messages can also be specified as argument."""
     if type(grasp) is Grasp:
         return self._g.pick(object_name, conversions.msg_to_string(grasp))
     else:
         return self._g.pick(object_name,
                             [conversions.msg_to_string(x) for x in grasp])
Пример #3
0
    def get_robot_markers(self, *args):
        """Get a MarkerArray of the markers that make up this robot

        Usage:
            (): get's all markers for current state
            state (RobotState): gets markers for a particular state
            values (dict): get markers with given values
            values, links (dict, list): get markers with given values and these links
            group (string):  get all markers for a group
            group, values (string, dict): get all markers for a group with desired values
        """
        mrkr = MarkerArray()
        if not args:
            conversions.msg_from_string(mrkr, self._r.get_robot_markers())
        else:
            if isinstance(args[0], RobotState):
                msg_str = conversions.msg_to_string(args[0])
                conversions.msg_from_string(mrkr, self._r.get_robot_markers(msg_str))
            elif isinstance(args[0], dict):
                conversions.msg_from_string(mrkr, self._r.get_robot_markers(*args))
            elif isinstance(args[0], str):
                conversions.msg_from_string(mrkr, self._r.get_group_markers(*args))
            else:
                raise MoveItCommanderException("Unexpected type")
        return mrkr
Пример #4
0
    def compute_cartesian_path(self,
                               waypoints,
                               eef_step,
                               jump_threshold,
                               avoid_collisions=True,
                               path_constraints=None):
        """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """
        if path_constraints:
            if type(path_constraints) is Constraints:
                constraints_str = conversions.msg_to_string(path_constraints)
            else:
                raise MoveItCommanderException(
                    "Unable to set path constraints, unknown constraint type "
                    + type(path_constraints))
            (ser_path, fraction) = self._g.compute_cartesian_path(
                [conversions.pose_to_list(p) for p in waypoints], eef_step,
                jump_threshold, avoid_collisions, constraints_str)
        else:
            (ser_path, fraction) = self._g.compute_cartesian_path(
                [conversions.pose_to_list(p) for p in waypoints], eef_step,
                jump_threshold, avoid_collisions)

        path = RobotTrajectory()
        path.deserialize(ser_path)
        return (path, fraction)
Пример #5
0
 def set_path_constraints(self, value):
     """ Specify the path constraints to be used (as read from the database) """
     if value == None:
         self.clear_path_constraints()
     else:
         if type(value) is Constraints:
             self._g.set_path_constraints_from_msg(conversions.msg_to_string(value))
         elif not self._g.set_path_constraints(value):
             raise MoveItCommanderException("Unable to set path constraints " + value)
Пример #6
0
 def set_path_constraints(self, value):
     """ Specify the path constraints to be used (as read from the database) """
     if value == None:
         self.clear_path_constraints()
     else:
         if type(value) is Constraints:
             self._g.set_path_constraints_from_msg(conversions.msg_to_string(value))
         elif not self._g.set_path_constraints(value):
             raise MoveItCommanderException("Unable to set path constraints " + value)
Пример #7
0
 def set_trajectory_constraints(self, value):
     """ Specify the trajectory constraints to be used """
     if value is None:
         self.clear_trajectory_constraints()
     else:
         if type(value) is TrajectoryConstraints:
             self._g.set_trajectory_constraints_from_msg(
                 conversions.msg_to_string(value))
         elif not self._g.set_trajectory_constraints(value):
             raise MoveItCommanderException(
                 "Unable to set trajectory constraints " + value)
Пример #8
0
 def place(self, object_name, location=None):
     """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided"""
     result = False
     if location is None:
         result = self._g.place(object_name)
     elif type(location) is PoseStamped:
         old = self.get_pose_reference_frame()
         self.set_pose_reference_frame(location.header.frame_id)
         result = self._g.place(object_name, conversions.pose_to_list(location.pose))
         self.set_pose_reference_frame(old)
     elif type(location) is Pose:
         result = self._g.place(object_name, conversions.pose_to_list(location))
     elif type(location) is PlaceLocation:
         result = self._g.place(object_name, conversions.msg_to_string(location))
     else:
         raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped or PlaceLocation object")
     return result
Пример #9
0
 def place(self, object_name, location=None):
     """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided"""
     result = False
     if location is None:
         result = self._g.place(object_name)
     elif type(location) is PoseStamped:
         old = self.get_pose_reference_frame()
         self.set_pose_reference_frame(location.header.frame_id)
         result = self._g.place(object_name, conversions.pose_to_list(location.pose))
         self.set_pose_reference_frame(old)
     elif type(location) is Pose:
         result = self._g.place(object_name, conversions.pose_to_list(location))
     elif type(location) is PlaceLocation:
         result = self._g.place(object_name, conversions.msg_to_string(location))
     else:
         raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped or PlaceLocation object")
     return result
 def set_start_state(self, msg):
     """
     Specify a start state for the group.
     Parameters
     ----------
     msg : moveit_msgs/RobotState
     Examples
     --------
     >>> from moveit_msgs.msg import RobotState
     >>> from sensor_msgs.msg import JointState
     >>> joint_state = JointState()
     >>> joint_state.header = Header()
     >>> joint_state.header.stamp = rospy.Time.now()
     >>> joint_state.name = ['joint_a', 'joint_b']
     >>> joint_state.position = [0.17, 0.34]
     >>> moveit_robot_state = RobotState()
     >>> moveit_robot_state.joint_state = joint_state
     >>> group.set_start_state(moveit_robot_state)
     """
     self._g.set_start_state(conversions.msg_to_string(msg))
Пример #11
0
 def execute(self, plan_msg, wait=True):
     """Execute a previously planned path"""
     if wait:
         return self._g.execute(conversions.msg_to_string(plan_msg))
     else:
         return self._g.async_execute(conversions.msg_to_string(plan_msg))
Пример #12
0
 def execute(self, plan_msg):
     """Execute a previously planned path"""
     return self._g.execute(conversions.msg_to_string(plan_msg))
Пример #13
0
    def set_joint_value_target(self, arg1, arg2=None, arg3=None):
        """
        Specify a target joint configuration for the group.
        - if the type of arg1 is one of the following: dict, list, JointState message, then no other arguments should be provided.
        The dict should specify pairs of joint variable names and their target values, the list should specify all the variable values
        for the group. The JointState message specifies the positions of some single-dof joints.
        - if the type of arg1 is string, then arg2 is expected to be defined and be either a real value or a list of real values. This is
        interpreted as setting a particular joint to a particular value.
        - if the type of arg1 is Pose or PoseStamped, both arg2 and arg3 could be defined. If arg2 or arg3 are defined, their types must
        be either string or bool. The string type argument is interpreted as the end-effector the pose is specified for (default is to use
        the default end-effector), and the bool is used to decide whether the pose specified is approximate (default is false). This situation
        allows setting the joint target of the group by calling IK. This does not send a pose to the planner and the planner will do no IK.
        Instead, one IK solution will be computed first, and that will be sent to the planner.
        """
        if isinstance(arg1, RobotState):
            if not self._g.set_state_value_target(
                    conversions.msg_to_string(arg1)):
                raise MoveItCommanderException(
                    "Error setting state target. Is the target state within bounds?"
                )

        elif isinstance(arg1, JointState):
            if (arg2 is not None or arg3 is not None):
                raise MoveItCommanderException("Too many arguments specified")
            if not self._g.set_joint_value_target_from_joint_state_message(
                    conversions.msg_to_string(arg1)):
                raise MoveItCommanderException(
                    "Error setting joint target. Is the target within bounds?")

        elif isinstance(arg1, str):
            if (arg2 is None):
                raise MoveItCommanderException(
                    "Joint value expected when joint name specified")
            if (arg3 is not None):
                raise MoveItCommanderException("Too many arguments specified")
            if not self._g.set_joint_value_target(arg1, arg2):
                raise MoveItCommanderException(
                    "Error setting joint target. Is the target within bounds?")

        elif isinstance(arg1, (Pose, PoseStamped)):
            approx = False
            eef = ""
            if (arg2 is not None):
                if type(arg2) is str:
                    eef = arg2
                else:
                    if type(arg2) is bool:
                        approx = arg2
                    else:
                        raise MoveItCommanderException("Unexpected type")
            if (arg3 is not None):
                if type(arg3) is str:
                    eef = arg3
                else:
                    if type(arg3) is bool:
                        approx = arg3
                    else:
                        raise MoveItCommanderException("Unexpected type")
            r = False
            if type(arg1) is PoseStamped:
                r = self._g.set_joint_value_target_from_pose_stamped(
                    conversions.msg_to_string(arg1), eef, approx)
            else:
                r = self._g.set_joint_value_target_from_pose(
                    conversions.msg_to_string(arg1), eef, approx)
            if not r:
                if approx:
                    raise MoveItCommanderException(
                        "Error setting joint target. Does your IK solver support approximate IK?"
                    )
                else:
                    raise MoveItCommanderException(
                        "Error setting joint target. Is IK running?")

        elif (hasattr(arg1, '__iter__')):
            if (arg2 is not None or arg3 is not None):
                raise MoveItCommanderException("Too many arguments specified")
            if not self._g.set_joint_value_target(arg1):
                raise MoveItCommanderException(
                    "Error setting joint target. Is the target within bounds?")

        else:
            raise MoveItCommanderException("Unsupported argument of type %s" %
                                           type(arg1))
Пример #14
0
 def set_start_state(self, msg):
     self._g.set_start_state(conversions.msg_to_string(msg))
Пример #15
0
 def pick(self, object_name, grasp = []):
     """Pick the named object. A grasp message, or a list of Grasp messages can also be specified as argument."""
     if type(grasp) is Grasp:
         return self._g.pick(object_name, conversions.msg_to_string(grasp))
     else:
         return self._g.pick(object_name, [conversions.msg_to_string(x) for x in grasp])
Пример #16
0
 def execute(self, plan_msg, wait = True):
     """Execute a previously planned path"""
     if wait:
         return self._g.execute(conversions.msg_to_string(plan_msg))
     else:
         return self._g.async_execute(conversions.msg_to_string(plan_msg))
Пример #17
0
 def execute(self, plan_msg):
     """Execute a previously planned path"""
     return self._g.execute(conversions.msg_to_string(plan_msg))
Пример #18
0
 def set_joint_value_target(self, arg1, arg2 = None, arg3 = None):
     """
     Specify a target joint configuration for the group.
     - if the type of arg1 is one of the following: dict, list, JointState message, then no other arguments should be provided.
     The dict should specify pairs of joint variable names and their target values, the list should specify all the variable values
     for the group. The JointState message specifies the positions of some single-dof joints. 
     - if the type of arg1 is string, then arg2 is expected to be defined and be either a real value or a list of real values. This is
     interpreted as setting a particular joint to a particular value.
     - if the type of arg1 is Pose or PoseStamped, both arg2 and arg3 could be defined. If arg2 or arg3 are defined, their types must
     be either string or bool. The string type argument is interpreted as the end-effector the pose is specified for (default is to use
     the default end-effector), and the bool is used to decide whether the pose specified is approximate (default is false). This situation
     allows setting the joint target of the group by calling IK. This does not send a pose to the planner and the planner will do no IK.
     Instead, one IK solution will be computed first, and that will be sent to the planner. 
     """
     if (type(arg1) is dict) or (type(arg1) is list):
         if (arg2 != None or arg3 != None):
             raise MoveItCommanderException("Too many arguments specified")
         if not self._g.set_joint_value_target(arg1):
             raise MoveItCommanderException("Error setting joint target. Is the target within bounds?")
         return
     if type(arg1) is JointState:
         if (arg2 != None or arg3 != None):
             raise MoveItCommanderException("Too many arguments specified")
         if not self._g.set_joint_value_target_from_joint_state_message(conversions.msg_to_string(arg1)):
             raise MoveItCommanderException("Error setting joint target. Is the target within bounds?")
     if (type(arg1) is str):
         if (arg2 == None):
             raise MoveItCommanderException("Joint value expected when joint name specified")
         if (arg3 != None):
             raise MoveItCommanderException("Too many arguments specified")
         if not self._g.set_joint_value_target(arg1, arg2):
             raise MoveItCommanderException("Error setting joint target. Is the target within bounds?")
     if (type(arg1) is PoseStamped) or (type(arg1) is Pose):
         approx = False
         eef = ""
         if (arg2 != None):
             if type(arg2) is str:
                 eef = arg2
             else:
                 if type(arg2) is bool:
                     approx = arg2
                 else:
                     raise MoveItCommanderException("Unexpected type")
         if (arg3 != None):
             if type(arg3) is str:
                 eef = arg3
             else:
                 if type(arg3) is bool:
                     approx = arg3
                 else:
                     raise MoveItCommanderException("Unexpected type")
         r = False
         if type(arg1) is PoseStamped:
             r = self._g.set_joint_value_target_from_pose_stamped(conversions.msg_to_string(arg1), eef, approx)
         else:
             r = self._g.set_joint_value_target_from_pose(conversions.msg_to_string(arg1), eef, approx)
         if not r:
             if approx:
                 raise MoveItCommanderException("Error setting joint target. Does your IK solver support approximate IK?")
             else:
                 raise MoveItCommanderException("Error setting joint target. Is IK running?")
Пример #19
0
 def set_start_state(self, msg):
     self._g.set_start_state(conversions.msg_to_string(msg))