def set_pose_targets(self, poses, end_effector_link = ""):
""" Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
if len(end_effector_link) > 0 or self.has_end_effector_link():
if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link):
raise MoveItCommanderException("Unable to set target poses")
else:
raise MoveItCommanderException("There is no end effector to set poses for")
def set_position_target(self, xyz, end_effector_link = ""):
""" Specify a target position for the end-effector. Any orientation of the end-effector is acceptable."""
if len(end_effector_link) > 0 or self.has_end_effector_link():
if not self._g.set_position_target(xyz[0], xyz[1], xyz[2], end_effector_link):
raise MoveItCommanderException("Unable to set position target")
else:
raise MoveItCommanderException("There is no end effector to set the pose for")
def set_orientation_target(self, q, end_effector_link = ""):
if len(end_effector_link) > 0 or self.has_end_effector_link():
if len(q) == 4:
self._g.set_orientation_target(q[0], q[1], q[2], q[3], end_effector_link)
else:
raise MoveItCommanderException("Expected [qx, qy, qz, qw]")
else:
raise MoveItCommanderException("There is no end effector to set the pose for")
def set_rpy_target(self, rpy, end_effector_link = ""):
if len(end_effector_link) > 0 or self.has_end_effector_link():
if len(rpy) == 3:
self._g.set_rpy_target(rpy[0], rpy[1], rpy[2], end_effector_link)
else:
raise MoveItCommanderException("Expected [roll, pitch, yaw]")
else:
raise MoveItCommanderException("There is no end effector to set the pose for")
def set_orientation_target(self, q, end_effector_link = ""):
""" Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
if len(end_effector_link) > 0 or self.has_end_effector_link():
if len(q) == 4:
if not self._g.set_orientation_target(q[0], q[1], q[2], q[3], end_effector_link):
raise MoveItCommanderException("Unable to set orientation target")
else:
raise MoveItCommanderException("Expected [qx, qy, qz, qw]")
else:
raise MoveItCommanderException("There is no end effector to set the pose for")
def set_rpy_target(self, rpy, end_effector_link = ""):
""" Specify a target orientation for the end-effector. Any position of the end-effector is acceptable."""
if len(end_effector_link) > 0 or self.has_end_effector_link():
if len(rpy) == 3:
if not self._g.set_rpy_target(rpy[0], rpy[1], rpy[2], end_effector_link):
raise MoveItCommanderException("Unable to set orientation target")
else:
raise MoveItCommanderException("Expected [roll, pitch, yaw]")
else:
raise MoveItCommanderException("There is no end effector to set the pose for")
def shift_pose_target(self, axis, value, end_effector_link = ""):
""" Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target """
if len(end_effector_link) > 0 or self.has_end_effector_link():
pose = self._g.get_current_pose(end_effector_link)
# by default we get orientation as a quaternion list
# if we are updating a rotation axis however, we convert the orientation to RPY
if axis > 2:
(r, p, y) = tf.transformations.euler_from_quaternion(pose[3:])
pose = [pose[0], pose[1], pose[2], r, p, y]
if axis >= 0 and axis < 6:
pose[axis] = pose[axis] + value
self.set_pose_target(pose, end_effector_link)
else:
raise MoveItCommanderException("An axis value between 0 and 5 expected")
else:
raise MoveItCommanderException("There is no end effector to set poses for")
def get_current_rpy(self, end_effector_link=""):
""" Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector. """
if len(end_effector_link) > 0 or self.has_end_effector_link():
return self._g.get_current_rpy(end_effector_link)
else:
raise MoveItCommanderException(
"There is no end effector to get the rpy of")
def set_max_acceleration_scaling_factor(self, value):
""" Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1]. """
if value > 0 and value <= 1:
self._g.set_max_acceleration_scaling_factor(value)
else:
raise MoveItCommanderException(
"Expected value in the range from 0 to 1 for scaling factor")
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 __make_mesh(self, name, pose, filename, scale = (1, 1, 1)):
co = CollisionObject()
scene = pyassimp.load(filename)
if not scene.meshes:
raise MoveItCommanderException("There are no meshes in the file")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
for face in scene.meshes[0].faces:
triangle = MeshTriangle()
if len(face.indices) == 3:
triangle.vertex_indices = [face.indices[0], face.indices[1], face.indices[2]]
mesh.triangles.append(triangle)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0]*scale[0]
point.y = vertex[1]*scale[1]
point.z = vertex[2]*scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def get_random_pose(self, end_effector_link=""):
if len(end_effector_link) > 0 or self.has_end_effector_link():
return conversions.list_to_pose_stamped(
self._g.get_random_pose(end_effector_link),
self.get_planning_frame())
else:
raise MoveItCommanderException(
"There is no end effector to get the pose of")
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 not self._g.set_path_constraints(value):
raise MoveItCommanderException(
"Unable to set path constraints " + value)
def __make_mesh(self, name, pose, filename, scale=(1, 1, 1)):
co = CollisionObject()
if pyassimp is False:
raise MoveItCommanderException(
"Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt"
)
scene = pyassimp.load(filename)
if not scene.meshes or len(scene.meshes) == 0:
raise MoveItCommanderException("There are no meshes in the file")
if len(scene.meshes[0].faces) == 0:
raise MoveItCommanderException("There are no faces in the mesh")
co.operation = CollisionObject.ADD
co.id = name
co.header = pose.header
mesh = Mesh()
first_face = scene.meshes[0].faces[0]
if hasattr(first_face, '__len__'):
for face in scene.meshes[0].faces:
if len(face) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [face[0], face[1], face[2]]
mesh.triangles.append(triangle)
elif hasattr(first_face, 'indices'):
for face in scene.meshes[0].faces:
if len(face.indices) == 3:
triangle = MeshTriangle()
triangle.vertex_indices = [
face.indices[0], face.indices[1], face.indices[2]
]
mesh.triangles.append(triangle)
else:
raise MoveItCommanderException(
"Unable to build triangles from mesh due to mesh object structure"
)
for vertex in scene.meshes[0].vertices:
point = Point()
point.x = vertex[0] * scale[0]
point.y = vertex[1] * scale[1]
point.z = vertex[2] * scale[2]
mesh.vertices.append(point)
co.meshes = [mesh]
co.mesh_poses = [pose.pose]
pyassimp.release(scene)
return co
def set_pose_target(self, pose, end_effector_link = ""):
""" Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:"""
""" [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
if len(end_effector_link) > 0 or self.has_end_effector_link():
ok = False
if type(pose) is PoseStamped:
old = self.get_pose_reference_frame()
self.set_pose_reference_frame(pose.header.frame_id)
ok = self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link)
self.set_pose_reference_frame(old)
elif type(pose) is Pose:
ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link)
else:
ok = self._g.set_pose_target(pose, end_effector_link)
if not ok:
raise MoveItCommanderException("Unable to set target pose")
else:
raise MoveItCommanderException("There is no end effector to set the pose for")
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)
def get_current_pose(self, end_effector_link=""):
""" Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector. """
if len(end_effector_link) > 0 or self.has_end_effector_link():
return conversions.list_to_pose_stamped(
self._g.get_current_pose(end_effector_link),
self.get_planning_frame())
else:
raise MoveItCommanderException(
"There is no end effector to get the pose of")
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_workspace(self, ws):
""" Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ] """
if len(ws) == 0:
self._g.set_workspace(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
else:
if len(ws) == 4:
self._g.set_workspace(ws[0], ws[1], 0.0, ws[2], ws[3], 0.0)
else:
if len(ws) == 6:
self._g.set_workspace(ws[0], ws[1], ws[2], ws[3], ws[4], ws[5])
else:
raise MoveItCommanderException("Expected 0, 4 or 6 values in list specifying workspace")
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_position_target(self, xyz, end_effector_link = ""):
if len(end_effector_link) > 0 or self.has_end_effector_link():
self._g.set_position_target(xyz[0], xyz[1], xyz[2], end_effector_link)
else:
raise MoveItCommanderException("There is no end effector to set the pose for")
def set_end_effector_link(self, link_name):
""" Set the name of the link to be considered as an end effector """
if not self._g.set_end_effector_link(link_name):
raise MoveItCommanderException("Unable to set end efector link")
def set_named_target(self, name):
if not self._g.set_named_target(name):
raise MoveItCommanderException("Unable to set target " + name)
def set_path_constraints(self, value):
if value == None:
self.clear_path_constraints()
else:
if not self._g.set_path_constraints(value):
raise MoveItCommanderException("Unable to set path constraints " + value)
def set_named_target(self, name):
""" Set a joint configuration by name. The name can be a name previlusy remembered with remember_joint_values() or a configuration specified in the SRDF. """
if not self._g.set_named_target(name):
raise MoveItCommanderException(
"Unable to set target %s. Is the target within bounds?" % name)
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))
def get_current_rpy(self, end_effector_link = ""):
if len(end_effector_link) > 0 or self.has_end_effector_link():
return self._g.get_current_rpy(end_effector_link)
else:
raise MoveItCommanderException("There is no end effector to get the rpy of")
