def __init__(self, node_name=u'giskard'):
giskard_topic = u'{}/command'.format(node_name)
if giskard_topic is not None:
self._client = SimpleActionClient(giskard_topic, MoveAction)
self._update_world_srv = rospy.ServiceProxy(
u'{}/update_world'.format(node_name), UpdateWorld)
self._get_object_names_srv = rospy.ServiceProxy(
u'{}/get_object_names'.format(node_name), GetObjectNames)
self._get_object_info_srv = rospy.ServiceProxy(
u'{}/get_object_info'.format(node_name), GetObjectInfo)
self._update_rviz_markers_srv = rospy.ServiceProxy(
u'{}/update_rviz_markers'.format(node_name), UpdateRvizMarkers)
self._get_attached_objects_srv = rospy.ServiceProxy(
u'{}/get_attached_objects'.format(node_name),
GetAttachedObjects)
self._marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(node_name))
self._client.wait_for_server()
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self._object_js_topics = {}
rospy.sleep(.3)
def get_attached_objects(self, req):
original_robot = URDFObject(self.get_robot().original_urdf)
link_names = self.get_robot().get_link_names()
original_link_names = original_robot.get_link_names()
attached_objects = list(set(link_names).difference(original_link_names))
attachment_points = [self.get_robot().get_parent_link_of_joint(object) for object in attached_objects]
res = GetAttachedObjectsResponse()
res.object_names = attached_objects
res.attachment_points = attachment_points
return res
def __init__(self, giskard_topic=u'giskardpy/command', ns=u'giskard'):
if giskard_topic is not None:
self.client = SimpleActionClient(giskard_topic, MoveAction)
self.update_world = rospy.ServiceProxy(u'{}/update_world'.format(ns), UpdateWorld)
self.marker_pub = rospy.Publisher(u'visualization_marker_array', MarkerArray, queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(ns))
self.client.wait_for_server()
self.tip_to_root = {}
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self.object_js_topics = {}
rospy.sleep(.3)
def load_urdf_string_into_bullet(urdf_string, pose=None):
"""
Loads a URDF string into the bullet world.
:param urdf_string: XML string of the URDF to load.
:type urdf_string: str
:param pose: Pose at which to load the URDF into the world.
:type pose: Pose
:return: internal PyBullet id of the loaded urdfs
:rtype: intload_urdf_string_into_bullet
"""
if pose is None:
pose = Pose()
pose.orientation.w = 1
if isinstance(pose, PoseStamped):
pose = pose.pose
object_name = URDFObject(urdf_string).get_name()
if object_name in get_body_names():
raise DuplicateNameException(
u'an object with name \'{}\' already exists in pybullet'.format(
object_name))
resolved_urdf = resolve_ros_iris_in_urdf(urdf_string)
filename = write_to_tmp(u'{}.urdfs'.format(random_string()), resolved_urdf)
with NullContextManager(
) if giskardpy.PRINT_LEVEL == DEBUG else suppress_stdout():
id = p.loadURDF(filename,
[pose.position.x, pose.position.y, pose.position.z], [
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w
],
flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
os.remove(filename)
return id
def __init__(self, sim=True, move=True):
# rospy.init_node('tests')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate', 'package://refills_perception_interface/owl/muh.owl')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate',
# 'package://refills_rooming_in/data/augsburg_rooming_in_map_2018-11-08_10-17-17.owl')
# rospy.set_param(DummyInterfaceNodeName + '/initial_beliefstate',
# 'package://refills_rooming_in/data/rooming_in_map_2018-11-29_16-59-38.owl')
# rospy.set_param(DummyInterfaceNodeName + '/path_to_json',
# 'package://refills_rooming_in/data/augsburg_rooming_in_map_2018-11-08_10-17-17.json')
self.query_shelf_systems_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_shelf_systems', QueryShelfSystems)
self.query_shelf_layers_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_shelf_layers', QueryShelfLayers)
self.query_facings_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_facings', QueryFacings)
self.finish_perception_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/finish_perception', FinishPerception)
self.query_reset_belief_state_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/reset_beliefstate', Trigger)
self.query_shelf_detection_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_detect_shelf_layers_path',
QueryDetectShelfLayersPath)
self.query_facing_detection_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_detect_facings_path',
QueryDetectFacingsPath)
self.query_product_counting_path_srv = rospy.ServiceProxy(
DummyInterfaceNodeName + '/query_count_products_posture',
QueryCountProductsPosture)
self.detect_shelves_ac = SimpleActionClient(
DummyInterfaceNodeName + '/detect_shelf_layers',
DetectShelfLayersAction)
self.detect_facings_ac = SimpleActionClient(
DummyInterfaceNodeName + '/detect_facings', DetectFacingsAction)
self.count_products_ac = SimpleActionClient(
DummyInterfaceNodeName + '/count_products', CountProductsAction)
# self.simple_base_goal_pub = rospy.Publisher('move_base_simple/goal', PoseStamped)
self.simple_joint_goal = rospy.ServiceProxy(
'refills_bot/set_joint_states', SetJointState)
self.sleep = sim
self.sleep_amount = 0
self.move = move
if URDFObject(rospy.get_param(
'robot_description')).get_name() == 'iai_donbot':
from refills_perception_interface.move_arm import MoveArm
self.giskard = MoveArm(avoid_self_collisinon=True)
else:
from refills_perception_interface.move_arm_kmr_iiwa import MoveArm
self.giskard = MoveArm(avoid_self_collisinon=True)
# self.base = MoveBase()
rospy.sleep(.5)
class GiskardWrapper(object):
def __init__(self, node_name=u'giskard'):
giskard_topic = u'{}/command'.format(node_name)
if giskard_topic is not None:
self._client = SimpleActionClient(giskard_topic, MoveAction)
self._update_world_srv = rospy.ServiceProxy(
u'{}/update_world'.format(node_name), UpdateWorld)
self._get_object_names_srv = rospy.ServiceProxy(
u'{}/get_object_names'.format(node_name), GetObjectNames)
self._get_object_info_srv = rospy.ServiceProxy(
u'{}/get_object_info'.format(node_name), GetObjectInfo)
self._update_rviz_markers_srv = rospy.ServiceProxy(
u'{}/update_rviz_markers'.format(node_name), UpdateRvizMarkers)
self._get_attached_objects_srv = rospy.ServiceProxy(
u'{}/get_attached_objects'.format(node_name),
GetAttachedObjects)
self._marker_pub = rospy.Publisher(u'visualization_marker_array',
MarkerArray,
queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(node_name))
self._client.wait_for_server()
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self._object_js_topics = {}
rospy.sleep(.3)
def get_robot_name(self):
"""
:rtype: str
"""
return self.robot_urdf.get_name()
def get_root(self):
"""
Returns the name of the robot's root link
:rtype: str
"""
return self.robot_urdf.get_root()
def get_robot_links(self):
"""
Returns a list of the robots links
:rtype: dict
"""
return self.robot_urdf.get_link_names()
def get_joint_states(self, topic=u'joint_states', timeout=1):
"""
Returns a dictionary of all joints (key) and their position (value)
:param topic: joint_state topic
:param timeout: duration to wait for JointState msg
:return: OrderedDict[str, float]
"""
try:
msg = rospy.wait_for_message(topic, JointState,
rospy.Duration(timeout))
return to_joint_state_position_dict(msg)
except rospy.ROSException:
rospy.logwarn("get_joint_states: wait_for_message timeout")
return {}
def set_cart_goal(self,
root_link,
tip_link,
goal_pose,
max_linear_velocity=None,
max_angular_velocity=None,
weight=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal pose
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose
:type goal_pose: PoseStamped
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
self.set_translation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_linear_velocity,
weight=weight)
self.set_rotation_goal(root_link,
tip_link,
goal_pose,
max_velocity=max_angular_velocity,
weight=weight)
def set_translation_goal(self,
root_link,
tip_link,
goal_pose,
weight=None,
max_velocity=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal position
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose, orientation will be ignored
:type goal_pose: PoseStamped
:param max_velocity: m/s, default 0.1
:type max_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
if not max_velocity and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.TRANSLATION_3D
constraint.root_link = str(root_link)
constraint.tip_link = str(tip_link)
constraint.goal = goal_pose
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianPosition'
params = {}
params[u'root_link'] = root_link
params[u'tip_link'] = tip_link
params[u'goal'] = convert_ros_message_to_dictionary(goal_pose)
if max_velocity:
params[u'max_velocity'] = max_velocity
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_rotation_goal(self,
root_link,
tip_link,
goal_pose,
weight=None,
max_velocity=None):
"""
This goal will use the kinematic chain between root and tip link to move tip link into the goal orientation
:param root_link: name of the root link of the kin chain
:type root_link: str
:param tip_link: name of the tip link of the kin chain
:type tip_link: str
:param goal_pose: the goal pose, position will be ignored
:type goal_pose: PoseStamped
:param max_velocity: rad/s, default 0.5
:type max_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
if not max_velocity and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.ROTATION_3D
constraint.root_link = str(root_link)
constraint.tip_link = str(tip_link)
constraint.goal = goal_pose
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianOrientationSlerp'
params = {}
params[u'root_link'] = root_link
params[u'tip_link'] = tip_link
params[u'goal'] = convert_ros_message_to_dictionary(goal_pose)
if max_velocity:
params[u'max_velocity'] = max_velocity
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_joint_goal(self, goal_state, weight=None, max_velocity=None):
"""
This goal will move the robots joint to the desired configuration.
:param goal_state: Can either be a joint state messages or a dict mapping joint name to position.
:type goal_state: Union[JointState, dict]
:param weight: default WEIGHT_BELOW_CA
:type weight: float
:param max_velocity: default is the default of the added joint goals
:type max_velocity: float
"""
if weight is None and max_velocity is None:
constraint = JointConstraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
constraint.goal_state = goal_state
else:
for joint_name, joint_position in goal_state.items():
constraint.goal_state.name.append(joint_name)
constraint.goal_state.position.append(joint_position)
self.cmd_seq[-1].joint_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = JointConstraint.JOINT
if isinstance(goal_state, JointState):
goal_state = goal_state
else:
goal_state2 = JointState()
for joint_name, joint_position in goal_state.items():
goal_state2.name.append(joint_name)
goal_state2.position.append(joint_position)
goal_state = goal_state2
params = {}
params[u'goal_state'] = convert_ros_message_to_dictionary(
goal_state)
if weight is not None:
params[u'weight'] = weight
if max_velocity is not None:
params[u'max_velocity'] = max_velocity
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def align_planes(self,
tip_link,
tip_normal,
root_link=None,
root_normal=None,
max_angular_velocity=None,
weight=WEIGHT_ABOVE_CA):
"""
This Goal will use the kinematic chain between tip and root normal to align both
:param root_link: name of the root link for the kinematic chain, default robot root link
:type root_link: str
:param tip_link: name of the tip link for the kinematic chain
:type tip_link: str
:param tip_normal: normal at the tip of the kin chain, default is z axis of robot root link
:type tip_normal: Vector3Stamped
:param root_normal: normal at the root of the kin chain
:type root_normal: Vector3Stamped
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
if root_link is None:
root_link = self.get_root()
if root_normal is None:
root_normal = Vector3Stamped()
root_normal.header.frame_id = self.get_root()
root_normal.vector.z = 1
params = {
u'tip_link': tip_link,
u'tip_normal': tip_normal,
u'root_link': root_link,
u'root_normal': root_normal
}
if weight is not None:
params[u'weight'] = weight
if max_angular_velocity is not None:
params[u'max_angular_velocity'] = max_angular_velocity
self.set_json_goal(u'AlignPlanes', **params)
def avoid_joint_limits(self, percentage=15, weight=WEIGHT_BELOW_CA):
"""
This goal will push joints away from their position limits
:param percentage: default 15, if limits are 0-100, the constraint will push into the 15-85 range
:type percentage: float
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
self.set_json_goal(u'AvoidJointLimits',
percentage=percentage,
weight=weight)
def limit_cartesian_velocity(self,
root_link,
tip_link,
weight=WEIGHT_ABOVE_CA,
max_linear_velocity=0.1,
max_angular_velocity=0.5,
hard=True):
"""
This goal will limit the cartesian velocity of the tip link relative to root link
:param root_link: root link of the kin chain
:type root_link: str
:param tip_link: tip link of the kin chain
:type tip_link: str
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param hard: default True, will turn this into a hard constraint, that will always be satisfied, can could
make some goal combination infeasible
:type hard: bool
"""
self.set_json_goal(u'CartesianVelocityLimit',
root_link=root_link,
tip_link=tip_link,
weight=weight,
max_linear_velocity=max_linear_velocity,
max_angular_velocity=max_angular_velocity,
hard=hard)
def grasp_bar(self,
root_link,
tip_link,
tip_grasp_axis,
bar_center,
bar_axis,
bar_length,
max_linear_velocity=0.1,
max_angular_velocity=0.5,
weight=WEIGHT_ABOVE_CA):
"""
This goal can be used to grasp bars. It's like a cartesian goal with some freedom along one axis.
:param root_link: root link of the kin chain
:type root_link: str
:param tip_link: tip link of the kin chain
:type tip_link: str
:param tip_grasp_axis: this axis of the tip will be aligned with bar_axis
:type tip_grasp_axis: Vector3Stamped
:param bar_center: center of the bar
:type bar_center: PointStamped
:param bar_axis: tip_grasp_axis will be aligned with this vector
:type bar_axis: Vector3Stamped
:param bar_length: length of the bar
:type bar_length: float
:param max_linear_velocity: m/s, default 0.1
:type max_linear_velocity: float
:param max_angular_velocity: rad/s, default 0.5
:type max_angular_velocity: float
:param weight: default WEIGHT_ABOVE_CA
:type weight: float
"""
self.set_json_goal(u'GraspBar',
root_link=root_link,
tip_link=tip_link,
tip_grasp_axis=tip_grasp_axis,
bar_center=bar_center,
bar_axis=bar_axis,
bar_length=bar_length,
max_linear_velocity=max_linear_velocity,
max_angular_velocity=max_angular_velocity,
weight=weight)
def set_pull_door_goal(self,
tip_link,
object_name_prefix,
object_link_name,
angle_goal,
weight=WEIGHT_ABOVE_CA):
"""
:type tip_link: str
:param tip_link: tip of manipulator (gripper) which is used
:type object_name_prefix: object name link prefix
:param object_name_prefix: string
:type object_link_name str
:param object_link_name name of the object link name
:type object_link_name str
:param object_link_name handle to grasp
:type angle_goal: float
:param angle_goal: how far to open
:type weight float
:param weight Default = WEIGHT_ABOVE_CA
"""
self.set_json_goal(u'OpenDoor',
tip_link=tip_link,
object_name=object_name_prefix,
object_link_name=object_link_name,
angle_goal=angle_goal,
weight=weight)
def update_god_map(self, updates):
"""
don't use, it's only for hacks :)
"""
self.set_json_goal(u'UpdateGodMap', updates=updates)
def pointing(self,
tip_link,
goal_point,
root_link=None,
pointing_axis=None,
weight=None):
"""
Uses the kinematic chain from root_link to tip_link to move the pointing axis, such that it points to the goal point.
:param tip_link: name of the tip of the kin chain
:type tip_link: str
:param goal_point: where the pointing_axis will point towards
:type goal_point: PointStamped
:param root_link: name of the root of the kin chain
:type root_link: str
:param pointing_axis: default is z axis, this axis will point towards the goal_point
:type pointing_axis: Vector3Stamped
:param weight: default WEIGHT_BELOW_CA
:type weight: float
"""
kwargs = {u'tip_link': tip_link, u'goal_point': goal_point}
if root_link is not None:
kwargs[u'root_link'] = root_link
if pointing_axis is not None:
kwargs[u'pointing_axis'] = pointing_axis
if weight is not None:
kwargs[u'weight'] = weight
kwargs[u'goal_point'] = goal_point
self.set_json_goal(u'Pointing', **kwargs)
def set_json_goal(self, constraint_type, **kwargs):
"""
Set a goal for any of the goals defined in Constraints.py
:param constraint_type: Name of the Goal
:type constraint_type: str
:param kwargs: maps constraint parameter names to values. Values should be float, str or ros messages.
:type kwargs: dict
"""
constraint = Constraint()
constraint.type = constraint_type
for k, v in kwargs.items():
if isinstance(v, Message):
kwargs[k] = convert_ros_message_to_dictionary(v)
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)
def set_collision_entries(self, collisions):
"""
Adds collision entries to the current goal
:param collisions: list of CollisionEntry
:type collisions: list
"""
self.cmd_seq[-1].collisions.extend(collisions)
def allow_collision(self,
robot_links=(CollisionEntry.ALL, ),
body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL, )):
"""
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def avoid_collision(self,
min_dist,
robot_links=(CollisionEntry.ALL, ),
body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL, )):
"""
:param min_dist: the distance giskard is trying to keep between specified links
:type min_dist: float
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.min_dist = min_dist
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def allow_all_collisions(self):
"""
Allows all collisions for next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def allow_self_collision(self):
"""
Allows the collision with itself for the next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def avoid_self_collision(self):
"""
Avoid collisions with itself for the next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def avoid_all_collisions(self, distance=0.05):
"""
Avoids all collisions for next goal. The distance will override anything from the config file.
If you don't want to override the distance, don't call this function. Avoid all is the default, if you don't
add any collision entries.
:param distance: the distance that giskard is trying to keep from all objects
:type distance: float
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = distance
self.set_collision_entries([collision_entry])
def add_cmd(self):
"""
Adds another command to the goal sequence. Any set goal calls will be added the this new goal.
This is used, if you want Giskard to plan multiple goals in succession.
"""
move_cmd = MoveCmd()
self.cmd_seq.append(move_cmd)
def clear_cmds(self):
"""
Removes all move commands from the current goal, collision entries are left untouched.
"""
self.cmd_seq = []
self.add_cmd()
def plan_and_execute(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.PLAN_AND_EXECUTE, wait)
def check_reachability(self, wait=True):
"""
Not implemented
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.CHECK_REACHABILITY, wait)
def plan(self, wait=True):
"""
Plans, but doesn't execute the goal. Useful, if you just want to look at the planning ghost.
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
return self.send_goal(MoveGoal.PLAN_ONLY, wait)
def send_goal(self, goal_type, wait=True):
goal = self._get_goal()
goal.type = goal_type
if wait:
self._client.send_goal_and_wait(goal)
return self._client.get_result()
else:
self._client.send_goal(goal)
def get_collision_entries(self):
return self.cmd_seq
def _get_goal(self):
goal = MoveGoal()
goal.cmd_seq = self.cmd_seq
goal.type = MoveGoal.PLAN_AND_EXECUTE
self.clear_cmds()
return goal
def interrupt(self):
"""
Stops any goal that Giskard is processing.
"""
self._client.cancel_goal()
def get_result(self, timeout=rospy.Duration()):
"""
Waits for giskardpy result and returns it. Only used when plan_and_execute was called with wait=False
:type timeout: rospy.Duration
:rtype: MoveResult
"""
self._client.wait_for_result(timeout)
return self._client.get_result()
def clear_world(self):
"""
Removes any objects and attached objects from Giskard's world and reverts the robots urdf to what it got from
the parameter server.
:rtype: UpdateWorldResponse
"""
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(),
False, PoseStamped())
return self._update_world_srv.call(req)
def remove_object(self, name):
"""
:param name:
:type name: str
:return:
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.name = str(name)
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False,
PoseStamped())
return self._update_world_srv.call(req)
def add_box(self,
name=u'box',
size=(1, 1, 1),
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param size: (x length, y length, z length) in m
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self._update_world_srv.call(req)
def add_sphere(self,
name=u'sphere',
size=1,
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param size: radius in m
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.SPHERE
object.shape.dimensions.append(size)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def add_mesh(self,
name=u'mesh',
mesh=u'',
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param mesh: path to the meshes location. e.g. package://giskardpy/test/urdfs/meshes/bowl_21.obj
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.MESH_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.mesh = mesh
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def add_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=u'map',
position=(0, 0, 0),
orientation=(0, 0, 0, 1),
pose=None):
"""
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:param height: in m
:type height: float
:param radius: in m
:type radius: float
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.CYLINDER
object.shape.dimensions = [0, 0]
object.shape.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = height
object.shape.dimensions[SolidPrimitive.CYLINDER_RADIUS] = radius
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self._update_world_srv.call(req)
def attach_box(self,
name=u'box',
size=None,
frame_id=None,
position=None,
orientation=None,
pose=None):
"""
Add a box to the world and attach it to the robot at frame_id.
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:type pose: PoseStamped
:param pose: pose of the box
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self._update_world_srv.call(req)
def attach_cylinder(self,
name=u'cylinder',
height=1,
radius=1,
frame_id=None,
position=None,
orientation=None):
"""
Add a cylinder to the world and attach it to the robot at frame_id.
If pose is used, frame_id, position and orientation are ignored.
:type name: str
:type height: int
:param height: height of the cylinder. Default = 1
:type radius: int
:param radius: radius of the cylinder. Default = 1
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
cylinder = make_world_body_cylinder(name, height, radius)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(
frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(
*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(
*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, True, pose)
return self._update_world_srv.call(req)
def attach_object(self, name, link_frame_id):
"""
Attach an already existing object at link_frame_id of the robot.
:type name: str
:param link_frame_id: name of a robot link
:type link_frame_id: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.rigidly_attached = True
req.body.name = name
req.pose.header.frame_id = link_frame_id
req.operation = UpdateWorldRequest.ADD
return self._update_world_srv.call(req)
def detach_object(self, object_name):
"""
Detach an object from the robot and add it back to the world.
Careful though, you could amputate an arm be accident!
:type object_name: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.body.name = object_name
req.operation = req.DETACH
return self._update_world_srv.call(req)
def add_urdf(self, name, urdf, pose, js_topic=u'', set_js_topic=None):
"""
Adds a urdf to the world
:param name: name it will have in the world
:type name: str
:param urdf: urdf as string, no path
:type urdf: str
:type pose: PoseStamped
:param js_topic: Giskard will listen on that topic for joint states and update the urdf accordingly
:type js_topic: str
:param set_js_topic: A topic that the python wrapper will use to set the urdf joint state.
If None, set_js_topic == js_topic
:type set_js_topic: str
:return: UpdateWorldResponse
"""
if set_js_topic is None:
set_js_topic = js_topic
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False,
pose)
if js_topic:
# FIXME publisher has to be removed, when object gets deleted
# FIXME there could be sync error, if objects get added/removed by something else
self._object_js_topics[name] = rospy.Publisher(set_js_topic,
JointState,
queue_size=10)
return self._update_world_srv.call(req)
def set_object_joint_state(self, object_name, joint_states):
"""
:type object_name: str
:param joint_states: joint state message or a dict that maps joint name to position
:type joint_states: Union[JointState, dict]
:return: UpdateWorldResponse
"""
if isinstance(joint_states, dict):
joint_states = position_dict_to_joint_states(joint_states)
self._object_js_topics[object_name].publish(joint_states)
def get_object_names(self):
"""
returns the names of every object in the world
:rtype: GetObjectNamesResponse
"""
return self._get_object_names_srv()
def get_object_info(self, name):
"""
returns the joint state, joint state topic and pose of the object with the given name
:type name: str
:rtype: GetObjectInfoResponse
"""
return self._get_object_info_srv(name)
def update_rviz_markers(self, object_names):
"""
republishes visualization markers for rviz
:type object_names: list
:rtype: UpdateRvizMarkersResponse
"""
return self._update_rviz_markers_srv(object_names)
def get_attached_objects(self):
"""
returns a list of all objects that are attached to the robot and the respective attachement points
:rtype: GetAttachedObjectsResponse
"""
return self._get_attached_objects_srv()
def dump_state_cb(self, data):
try:
path = self.get_god_map().unsafe_get_data(identifier.data_folder)
folder_name = datetime.now().strftime('%Y-%m-%d-%H-%M-%S')
folder_path = '{}{}'.format(path, folder_name)
os.mkdir(folder_path)
robot = self.unsafe_get_robot()
world = self.unsafe_get_world()
with open("{}/dump.txt".format(folder_path), u'w') as f:
tree_manager = self.get_god_map().unsafe_get_data(identifier.tree_manager) # type: TreeManager
joint_state_message = tree_manager.get_node(u'js1').lock.get()
f.write("initial_robot_joint_state_dict = ")
write_dict(to_joint_state_position_dict(joint_state_message), f)
f.write("try:\n" +
" x_joint = initial_robot_joint_state_dict[\"odom_x_joint\"]\n" +
" y_joint = initial_robot_joint_state_dict[\"odom_y_joint\"]\n" +
" z_joint = initial_robot_joint_state_dict[\"odom_z_joint\"]\n" +
" base_pose = PoseStamped()\n" +
" base_pose.header.frame_id = \"map\"\n" +
" base_pose.pose.position = Point(x_joint, y_joint, 0)\n" +
" base_pose.pose.orientation = Quaternion(*quaternion_about_axis(z_joint, [0, 0, 1]))\n" +
" zero_pose.teleport_base(base_pose)\n" +
"except:\n" +
" logging.loginfo(\'no x,y and z joint\')\n\n")
f.write("zero_pose.send_and_check_joint_goal(initial_robot_joint_state_dict)\n")
robot_base_pose = PoseStamped()
robot_base_pose.header.frame_id = 'map'
robot_base_pose.pose = robot.base_pose
f.write("map_odom_transform_dict = ")
write_dict(convert_ros_message_to_dictionary(robot_base_pose), f)
f.write("map_odom_pose_stamped = convert_dictionary_to_ros_message(\'geometry_msgs/PoseStamped\', map_odom_transform_dict)\n")
f.write("map_odom_transform = Transform()\n" +
"map_odom_transform.rotation = map_odom_pose_stamped.pose.orientation\n" +
"map_odom_transform.translation = map_odom_pose_stamped.pose.position\n\n")
f.write("set_odom_map_transform = rospy.ServiceProxy('/map_odom_transform_publisher/update_map_odom_transform', UpdateTransform)\n")
f.write("set_odom_map_transform(map_odom_transform)\n")
original_robot = URDFObject(robot.original_urdf)
link_names = robot.get_link_names()
original_link_names = original_robot.get_link_names()
attached_objects = list(set(link_names).difference(original_link_names))
for object_name in attached_objects:
parent = robot.get_parent_link_of_joint(object_name)
pose = robot.get_fk_pose(parent, object_name)
world_object = robot.get_sub_tree_at_joint(object_name)
f.write("#attach {}\n".format(object_name))
with open("{}/{}.urdf".format(folder_path, object_name), u'w') as f_urdf:
f_urdf.write(world_object.original_urdf)
f.write('with open(\'{}/{}.urdf\', \"r\") as f:\n'.format(folder_path, object_name))
f.write(" {}_urdf = f.read()\n".format(object_name))
f.write("{0}_name = \"{0}\"\n".format(object_name))
f.write("{}_pose_stamped_dict = ".format(object_name))
write_dict(convert_ros_message_to_dictionary(pose), f)
f.write("{0}_pose_stamped = convert_dictionary_to_ros_message('geometry_msgs/PoseStamped', {0}_pose_stamped_dict)\n".format(object_name))
f.write(
"zero_pose.add_urdf(name={0}_name, urdf={0}_urdf, pose={0}_pose_stamped)\n".format(object_name))
f.write(
"zero_pose.attach_existing(name={0}_name, frame_id=\'{1}\')\n".format(object_name, parent))
for object_name, world_object in world.get_objects().items(): # type: (str, WorldObject)
f.write("#add {}\n".format(object_name))
with open("{}/{}.urdf".format(folder_path, object_name), u'w') as f_urdf:
f_urdf.write(world_object.original_urdf)
f.write('with open(\'{}/{}.urdf\', \"r\") as f:\n'.format(folder_path,object_name))
f.write(" {}_urdf = f.read()\n".format(object_name))
f.write("{0}_name = \"{0}\"\n".format(object_name))
f.write("{0}_js_topic = \"{0}_js_topic\"\n".format(object_name))
f.write("{}_pose_dict = ".format(object_name))
write_dict(convert_ros_message_to_dictionary(world_object.base_pose), f)
f.write("{0}_pose = convert_dictionary_to_ros_message('geometry_msgs/Pose', {0}_pose_dict)\n".format(object_name))
f.write("{}_pose_stamped = PoseStamped()\n".format(object_name))
f.write("{0}_pose_stamped.pose = {0}_pose\n".format(object_name))
f.write("{0}_pose_stamped.header.frame_id = \"map\"\n".format(object_name))
f.write("zero_pose.add_urdf(name={0}_name, urdf={0}_urdf, pose={0}_pose_stamped, js_topic={0}_js_topic, set_js_topic=None)\n".format(object_name))
f.write("{}_joint_state = ".format(object_name))
write_dict(to_joint_state_position_dict((dict_to_joint_states(world_object.joint_state))), f)
f.write("zero_pose.set_object_joint_state({0}_name, {0}_joint_state)\n\n".format(object_name))
last_goal = self.get_god_map().unsafe_get_data(identifier.next_move_goal)
if last_goal:
f.write(u'last_goal_dict = ')
write_dict(convert_ros_message_to_dictionary(last_goal), f)
f.write("last_goal_msg = convert_dictionary_to_ros_message('giskard_msgs/MoveCmd', last_goal_dict)\n")
f.write("last_action_goal = MoveActionGoal()\n")
f.write("last_move_goal = MoveGoal()\n")
f.write("last_move_goal.cmd_seq = [last_goal_msg]\n")
f.write("last_move_goal.type = MoveGoal.PLAN_AND_EXECUTE\n")
f.write("last_action_goal.goal = last_move_goal\n")
f.write("zero_pose.send_and_check_goal(goal=last_action_goal)\n")
else:
f.write(u'#no goal\n')
logging.loginfo(u'saved dump to {}'.format(folder_path))
except:
logging.logerr('failed to dump state pls try again')
res = TriggerResponse()
res.message = 'failed to dump state pls try again'
return TriggerResponse()
res = TriggerResponse()
res.success = True
res.message = 'saved dump to {}'.format(folder_path)
return res
class GiskardWrapper(object):
def __init__(self, giskard_topic=u'giskardpy/command', ns=u'giskard'):
if giskard_topic is not None:
self.client = SimpleActionClient(giskard_topic, MoveAction)
self.update_world = rospy.ServiceProxy(u'{}/update_world'.format(ns), UpdateWorld)
self.marker_pub = rospy.Publisher(u'visualization_marker_array', MarkerArray, queue_size=10)
rospy.wait_for_service(u'{}/update_world'.format(ns))
self.client.wait_for_server()
self.tip_to_root = {}
self.robot_urdf = URDFObject(rospy.get_param(u'robot_description'))
self.collisions = []
self.clear_cmds()
self.object_js_topics = {}
rospy.sleep(.3)
def get_robot_name(self):
return self.robot_urdf.get_name()
def get_root(self):
return self.robot_urdf.get_root()
def set_cart_goal(self, root, tip, pose_stamped, trans_max_speed=None, rot_max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
self.set_translation_goal(root, tip, pose_stamped, max_speed=trans_max_speed)
self.set_rotation_goal(root, tip, pose_stamped, max_speed=rot_max_speed)
def set_translation_goal(self, root, tip, pose_stamped, weight=None, gain=None, max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
if not gain and not max_speed and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.TRANSLATION_3D
constraint.root_link = str(root)
constraint.tip_link = str(tip)
constraint.goal = pose_stamped
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianPosition'
params = {}
params[u'root_link'] = root
params[u'tip_link'] = tip
params[u'goal'] = convert_ros_message_to_dictionary(pose_stamped)
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_rotation_goal(self, root, tip, pose_stamped, weight=None, gain=None, max_speed=None):
"""
:param tip:
:type tip: str
:param pose_stamped:
:type pose_stamped: PoseStamped
"""
if not gain and not max_speed and not weight:
constraint = CartesianConstraint()
constraint.type = CartesianConstraint.ROTATION_3D
constraint.root_link = str(root)
constraint.tip_link = str(tip)
constraint.goal = pose_stamped
self.cmd_seq[-1].cartesian_constraints.append(constraint)
else:
constraint = Constraint()
constraint.type = u'CartesianOrientationSlerp'
params = {}
params[u'root_link'] = root
params[u'tip_link'] = tip
params[u'goal'] = convert_ros_message_to_dictionary(pose_stamped)
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
if weight:
params[u'weight'] = weight
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def set_joint_goal(self, joint_state, weight=None, gain=None, max_speed=None):
"""
:param joint_state:
:type joint_state: dict
"""
if not weight and not gain and not max_speed:
constraint = JointConstraint()
constraint.type = JointConstraint.JOINT
if isinstance(joint_state, dict):
for joint_name, joint_position in joint_state.items():
constraint.goal_state.name.append(joint_name)
constraint.goal_state.position.append(joint_position)
elif isinstance(joint_state, JointState):
constraint.goal_state = joint_state
self.cmd_seq[-1].joint_constraints.append(constraint)
elif isinstance(joint_state, dict):
for joint_name, joint_position in joint_state.items():
constraint = Constraint()
constraint.type = u'JointPosition'
params = {}
params[u'joint_name'] = joint_name
params[u'goal'] = joint_position
if weight:
params[u'weight'] = weight
if gain:
params[u'gain'] = gain
if max_speed:
params[u'max_speed'] = max_speed
constraint.parameter_value_pair = json.dumps(params)
self.cmd_seq[-1].constraints.append(constraint)
def align_planes(self, tip, tip_normal, root=None, root_normal=None):
"""
:type tip: str
:type tip_normal: Vector3Stamped
:type root: str
:type root_normal: Vector3Stamped
:return:
"""
root = root if root else self.get_root()
tip_normal = convert_ros_message_to_dictionary(tip_normal)
if not root_normal:
root_normal = Vector3Stamped()
root_normal.header.frame_id = self.get_root()
root_normal.vector.z = 1
root_normal = convert_ros_message_to_dictionary(root_normal)
self.set_json_goal(u'AlignPlanes', tip=tip, tip_normal=tip_normal, root=root, root_normal=root_normal)
def gravity_controlled_joint(self, joint_name, object_name):
self.set_json_goal(u'GravityJoint', joint_name=joint_name, object_name=object_name)
def update_god_map(self, updates):
self.set_json_goal(u'UpdateGodMap', updates=updates)
def update_yaml(self, updates):
self.set_json_goal(u'UpdateYaml', updates=updates)
def pointing(self, tip, goal_point, root=None, pointing_axis=None, weight=None):
kwargs = {u'tip':tip,
u'goal_point':goal_point}
if root is not None:
kwargs[u'root'] = root
if pointing_axis is not None:
kwargs[u'pointing_axis'] = convert_ros_message_to_dictionary(pointing_axis)
if weight is not None:
kwargs[u'weight'] = weight
kwargs[u'goal_point'] = convert_ros_message_to_dictionary(goal_point)
self.set_json_goal(u'Pointing', **kwargs)
def set_json_goal(self, constraint_type, **kwargs):
constraint = Constraint()
constraint.type = constraint_type
for k, v in kwargs.items():
if isinstance(v, Message):
kwargs[k] = convert_ros_message_to_dictionary(v)
constraint.parameter_value_pair = json.dumps(kwargs)
self.cmd_seq[-1].constraints.append(constraint)
def set_collision_entries(self, collisions):
self.cmd_seq[-1].collisions.extend(collisions)
def allow_collision(self, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL,)):
"""
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def avoid_collision(self, min_dist, robot_links=(CollisionEntry.ALL,), body_b=CollisionEntry.ALL,
link_bs=(CollisionEntry.ALL,)):
"""
:param min_dist: the distance giskard is trying to keep between specified links
:type min_dist: float
:param robot_links: list of robot link names as str, None or empty list means all
:type robot_links: list
:param body_b: name of the other body, use the robots name to modify self collision behavior, empty string means all bodies
:type body_b: str
:param link_bs: list of link name of body_b, None or empty list means all
:type link_bs: list
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.min_dist = min_dist
collision_entry.robot_links = [str(x) for x in robot_links]
collision_entry.body_b = str(body_b)
collision_entry.link_bs = [str(x) for x in link_bs]
self.set_collision_entries([collision_entry])
def allow_all_collisions(self):
"""
Allows all collisions for next goal.
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def allow_self_collision(self):
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.ALLOW_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
self.set_collision_entries([collision_entry])
def set_self_collision_distance(self, min_dist=0.05):
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = self.get_robot_name()
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = min_dist
self.set_collision_entries([collision_entry])
def avoid_all_collisions(self, distance=0.05):
"""
Avoids all collisions for next goal.
:param distance: the distance that giskard is trying to keep from all objects
:type distance: float
"""
collision_entry = CollisionEntry()
collision_entry.type = CollisionEntry.AVOID_COLLISION
collision_entry.robot_links = [CollisionEntry.ALL]
collision_entry.body_b = CollisionEntry.ALL
collision_entry.link_bs = [CollisionEntry.ALL]
collision_entry.min_dist = distance
self.set_collision_entries([collision_entry])
def add_cmd(self, max_trajectory_length=20):
move_cmd = MoveCmd()
# move_cmd.max_trajectory_length = max_trajectory_length
self.cmd_seq.append(move_cmd)
def clear_cmds(self):
"""
Removes all move commands from the current goal, collision entries are left untouched.
"""
self.cmd_seq = []
self.add_cmd()
def plan_and_execute(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
goal = self._get_goal()
if wait:
self.client.send_goal_and_wait(goal)
return self.client.get_result()
else:
self.client.send_goal(goal)
def plan(self, wait=True):
"""
:param wait: this function block if wait=True
:type wait: bool
:return: result from giskard
:rtype: MoveResult
"""
goal = self._get_goal()
goal.type = MoveGoal.PLAN_ONLY
if wait:
self.client.send_goal_and_wait(goal)
return self.client.get_result()
else:
self.client.send_goal(goal)
def get_collision_entries(self):
return self.cmd_seq
def _get_goal(self):
goal = MoveGoal()
goal.cmd_seq = self.cmd_seq
goal.type = MoveGoal.PLAN_AND_EXECUTE
self.clear_cmds()
return goal
def interrupt(self):
self.client.cancel_goal()
def get_result(self, timeout=rospy.Duration()):
"""
Waits for giskardpy result and returns it. Only used when plan_and_execute was called with wait=False
:type timeout: rospy.Duration
:rtype: MoveResult
"""
self.client.wait_for_result(timeout)
return self.client.get_result()
def clear_world(self):
"""
:rtype: UpdateWorldResponse
"""
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE_ALL, WorldBody(), False, PoseStamped())
return self.update_world.call(req)
def remove_object(self, name):
"""
:param name:
:type name: str
:return:
:rtype: UpdateWorldResponse
"""
object = WorldBody()
object.name = str(name)
req = UpdateWorldRequest(UpdateWorldRequest.REMOVE, object, False, PoseStamped())
return self.update_world.call(req)
def add_box(self, name=u'box', size=(1, 1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
"""
:param name:
:param size: (x length, y length, z length)
:param frame_id:
:param position:
:param orientation:
:param pose:
:return:
"""
box = make_world_body_box(name, size[0], size[1], size[2])
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, False, pose)
return self.update_world.call(req)
def add_sphere(self, name=u'sphere', size=1, frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.SPHERE
object.shape.dimensions.append(size)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def add_mesh(self, name=u'mesh', mesh=u'', frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
object = WorldBody()
object.type = WorldBody.MESH_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.mesh = mesh
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def add_cylinder(self, name=u'cylinder', size=(1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1),
pose=None):
object = WorldBody()
object.type = WorldBody.PRIMITIVE_BODY
object.name = str(name)
if pose is None:
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id)
pose.pose.position = Point(*position)
pose.pose.orientation = Quaternion(*orientation)
object.shape.type = SolidPrimitive.CYLINDER
object.shape.dimensions.append(size[0])
object.shape.dimensions.append(size[1])
req = UpdateWorldRequest(UpdateWorldRequest.ADD, object, False, pose)
return self.update_world.call(req)
def attach_box(self, name=u'box', size=None, frame_id=None, position=None, orientation=None):
"""
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
box = make_world_body_box(name, size[0], size[1], size[2])
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, box, True, pose)
return self.update_world.call(req)
def attach_cylinder(self, name=u'cylinder', height=1, radius=1, frame_id=None, position=None, orientation=None):
"""
:type name: str
:type size: list
:type frame_id: str
:type position: list
:type orientation: list
:rtype: UpdateWorldResponse
"""
cylinder = make_world_body_cylinder(name, height, radius)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = str(frame_id) if frame_id is not None else u'map'
pose.pose.position = Point(*(position if position is not None else [0, 0, 0]))
pose.pose.orientation = Quaternion(*(orientation if orientation is not None else [0, 0, 0, 1]))
req = UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, True, pose)
return self.update_world.call(req)
def attach_object(self, name, link_frame_id):
"""
:type name: str
:type link_frame_id: str
:return: UpdateWorldResponse
"""
req = UpdateWorldRequest()
req.rigidly_attached = True
req.body.name = name
req.pose.header.frame_id = link_frame_id
req.operation = UpdateWorldRequest.ADD
return self.update_world.call(req)
def detach_object(self, object_name):
req = UpdateWorldRequest()
req.body.name = object_name
req.operation = req.DETACH
return self.update_world.call(req)
def add_urdf(self, name, urdf, pose, js_topic=u''):
urdf_body = WorldBody()
urdf_body.name = str(name)
urdf_body.type = WorldBody.URDF_BODY
urdf_body.urdf = str(urdf)
urdf_body.joint_state_topic = str(js_topic)
req = UpdateWorldRequest(UpdateWorldRequest.ADD, urdf_body, False, pose)
if js_topic:
# FIXME publisher has to be removed, when object gets deleted
# FIXME there could be sync error, if objects get added/removed by something else
self.object_js_topics[name] = rospy.Publisher(js_topic, JointState, queue_size=10)
return self.update_world.call(req)
def set_object_joint_state(self, object_name, joint_states):
if isinstance(joint_states, dict):
joint_states = dict_to_joint_states(joint_states)
self.object_js_topics[object_name].publish(joint_states)
while not rospy.has_param('/urdf_merger/urdf_sources'):
logging.loginfo('waiting for rosparam /urdf_merger/urdf_sources')
rospy.sleep(1.0)
while not rospy.has_param('/urdf_merger/robot_name'):
logging.loginfo('waiting for rosparam /urdf_merger/robot_name')
rospy.sleep(1.0)
urdf_sources = rospy.get_param('/urdf_merger/urdf_sources')
robot_name = rospy.get_param('/urdf_merger/robot_name')
merged_object = None
if rospy.has_param(urdf_sources[0]['source']):
urdf_string = rospy.get_param(urdf_sources[0]['source'])
merged_object = URDFObject(urdf_string)
else:
merged_object = URDFObject.from_urdf_file(urdf_sources[0]['source'])
for source in urdf_sources[1:]:
if rospy.has_param(source['source']):
urdf_string = rospy.get_param(source['source'])
urdf_object = URDFObject(urdf_string)
else:
urdf_object = URDFObject.from_urdf_file(source['source'])
positon = Point()
rotation = Quaternion()
if 'position' in source.keys():
position = Point(source['position'][0], source['position'][1],
source['position'][2])