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 list_to_pose(pose_list):
pose_msg = Pose()
if len(pose_list) == 7:
pose_msg.position.x = pose_list[0]
pose_msg.position.y = pose_list[1]
pose_msg.position.z = pose_list[2]
pose_msg.orientation.x = pose_list[3]
pose_msg.orientation.y = pose_list[4]
pose_msg.orientation.z = pose_list[5]
pose_msg.orientation.w = pose_list[6]
elif len(pose_list) == 6:
pose_msg.position.x = pose_list[0]
pose_msg.position.y = pose_list[1]
pose_msg.position.z = pose_list[2]
q = tf.transformations.quaternion_from_euler(pose_list[3],
pose_list[4],
pose_list[5])
pose_msg.orientation.x = q[0]
pose_msg.orientation.y = q[1]
pose_msg.orientation.z = q[2]
pose_msg.orientation.w = q[3]
else:
raise MoveItCommanderException(
"Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)"
)
return pose_msg
def list_to_pose_stamped(pose_list, target_frame):
pose_msg = PoseStamped()
if len(pose_list) == 7:
pose_msg.pose.position.x = pose_list[0]
pose_msg.pose.position.y = pose_list[1]
pose_msg.pose.position.z = pose_list[2]
pose_msg.pose.orientation.x = pose_list[3]
pose_msg.pose.orientation.y = pose_list[4]
pose_msg.pose.orientation.z = pose_list[5]
pose_msg.pose.orientation.w = pose_list[6]
elif len(pose_list) == 6:
pose_msg.pose.position.x = pose_list[0]
pose_msg.pose.position.y = pose_list[1]
pose_msg.pose.position.z = pose_list[2]
q = tf.transformations.quaternion_from_euler(pose_list[3],
pose_list[4],
pose_list[5])
pose_msg.pose.orientation.x = q[0]
pose_msg.pose.orientation.y = q[1]
pose_msg.pose.orientation.z = q[2]
pose_msg.pose.orientation.w = q[3]
else:
raise MoveItCommanderException(
"Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)"
)
pose_msg.header.frame_id = target_frame
pose_msg.header.stamp = rospy.Time.now()
return pose_msg
def get_group(self, name):
if not self._groups.has_key(name):
if not self.has_group(name):
raise MoveItCommanderException("There is no group named %s" %
name)
self._groups[name] = MoveGroupCommander(name)
return self._groups[name]
def get_group(self, name):
"""
@param name str: Name of movegroup
@rtype: moveit_commander.MoveGroupCommander
"""
if not self._groups.has_key(name):
if not self.has_group(name):
raise MoveItCommanderException("There is no group named %s" % name)
self._groups[name] = MoveGroupCommander(name, self._robot_description, self._ns)
return self._groups[name]
def get_link(self, name):
"""
@param name str: Name of movegroup
@rtype: moveit_commander.robot.Link
@raise exception: MoveItCommanderException
"""
if name in self.get_link_names():
return self.Link(self, name)
else:
raise MoveItCommanderException("There is no link named %s" % name)
def get_link_names(self, group=None):
"""Get the links that make up a group. If no group name is specified, all the links in the robot model are returned. """
if group is not None:
if self.has_group(group):
return self._r.get_group_link_names(group)
else:
raise MoveItCommanderException("There is no group named %s" %
group)
else:
return self._r.get_link_names()
def get_joint_names(self, group=None):
"""Get the names of all the movable joints that make up a group (mimic joints and fixed joints are excluded). If no group name is specified, all joints in the robot model are returned, including fixed and mimic joints """
if group is not None:
if self.has_group(group):
return self._r.get_group_joint_names(group)
else:
raise MoveItCommanderException("There is no group named %s" %
group)
else:
return self._r.get_joint_names()
def list_to_position(position_list):
position_msg = Point()
if len(position_list) == 3:
position_msg.x = position_list[0]
position_msg.y = position_list[1]
position_msg.z = position_list[2]
else:
raise MoveItCommanderException("Expected 3 elements in list: (x,y,z)")
return position_msg
def move(self, position, wait=True):
group = self._robot.get_default_owner_group()
if group is None:
raise MoveItCommanderException(
"There is no known group containing joint %s. Cannot move."
% self._name)
gc = self._robot.get_group(group)
if gc is not None:
gc.set_joint_value_target(gc.get_current_joint_values())
gc.set_joint_value_target(self._name, position)
return gc.go(wait)
return False
def move(self, position, wait=True):
"""
@param position [float]: List of joint angles to achieve.
@param wait bool: If false, the commands gets operated asynchronously.
"""
group = self._robot.get_default_owner_group(self.name())
if group is None:
raise MoveItCommanderException("There is no known group containing joint %s. Cannot move." % self._name)
gc = self._robot.get_group(group)
if gc is not None:
gc.set_joint_value_target(gc.get_current_joint_values())
gc.set_joint_value_target(self._name, position)
return gc.go(wait)
return False
def pose_to_lists(pose_msg, orientation_type):
"""
if orientation_type == euler, retrun euler angles in radians
"""
# print(orientation_type)
pose = pose_to_list(pose_msg)
position = pose[0:3]
quaternion = pose[3:7]
if orientation_type == "euler":
euler = euler_from_quaternion(quaternion)
orientation = euler
elif orientation_type == "quaternion":
orientation = quaternion
else:
raise MoveItCommanderException(
"Unknown type, accepts type 'euler' or 'quaternion'")
return position, orientation
def list_to_orientation(orientation_list):
if len(orientation_list) == 3:
quat_list = quaternion_from_euler(orientation_list[0],
orientation_list[1],
orientation_list[2])
elif len(orientation_list) == 4:
quat_list = orientation_list
else:
raise MoveItCommanderException(
"Expected orinetation list containing 3 (x, y, z) or 4 (x, y, z, w) elements"
)
orientation_msg = Quaternion()
orientation_msg.x = quat_list[0]
orientation_msg.y = quat_list[1]
orientation_msg.z = quat_list[2]
orientation_msg.w = quat_list[3]
return orientation_msg
def get_link(self, name):
if name in self.get_link_names():
return self.Link(self, name)
else:
raise MoveItCommanderException("There is no link named %s" % name)
def get_joint(self, name):
if name in self.get_joint_names():
return self.Joint(self, name)
else:
raise MoveItCommanderException("There is no joint named %s" % name)