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
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
def place(self, object_name, location=None, plan_only=False):
"""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 not location:
result = self._g.place(object_name, plan_only)
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), plan_only)
self.set_pose_reference_frame(old)
elif type(location) is Pose:
result = self._g.place(object_name, conversions.pose_to_list(location), plan_only)
elif type(location) is PlaceLocation:
result = self._g.place(object_name, conversions.msg_to_string(location), plan_only)
elif type(location) is list:
if location:
if type(location[0]) is PlaceLocation:
result = self._g.place_locations_list(object_name, [conversions.msg_to_string(x) for x in location], plan_only)
elif type(location[0]) is PoseStamped:
result = self._g.place_poses_list(object_name, [conversions.msg_to_string(x) for x in location], plan_only)
else:
raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object")
else:
raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object")
return result
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, acceleration_scaling_factor=1.0, algorithm="iterative_time_parameterization"):
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, acceleration_scaling_factor, algorithm)
traj_out = RobotTrajectory()
traj_out.deserialize(ser_traj_out)
return traj_out
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])
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
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
def enforce_bounds(self, robot_state_msg):
""" Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds() """
s = RobotState()
c_str = self._g.enforce_bounds(
conversions.msg_to_string(robot_state_msg))
conversions.msg_from_string(s, c_str)
return s
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)
def set_path_constraints(self, value):
""" Specify the path constraints to be used (as read from the database) """
if value is 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)
def set_path_constraints(self, value):
""" Specify the path constraints to be used (as read from the database) """
if value is 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)
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)
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)
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)
def place(self, object_name, location=None, plan_only=False):
"""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, plan_only)
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), plan_only)
self.set_pose_reference_frame(old)
elif type(location) is Pose:
result = self._g.place(object_name, conversions.pose_to_list(location), plan_only)
elif type(location) is PlaceLocation:
result = self._g.place(object_name, conversions.msg_to_string(location), plan_only)
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))
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))
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))
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 the IK solver functional?"
)
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))
def pick(self, object_name, grasp=[], plan_only=False):
"""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), plan_only)
else:
return self._g.pick(object_name, [conversions.msg_to_string(x) for x in grasp], plan_only)
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))
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))
