def attach_object(self, req): """ :type req: UpdateWorldRequest """ # assumes that parent has god map lock if self.unsafe_get_world().has_object(req.body.name): p = PoseStamped() p.header.frame_id = self.map_frame p.pose = self.unsafe_get_world().get_object(req.body.name).base_pose p = transform_pose(req.pose.header.frame_id, p) world_object = self.unsafe_get_world().get_object(req.body.name) self.unsafe_get_world().attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose) m = world_object.as_marker_msg() m.header.frame_id = p.header.frame_id m.pose = p.pose else: world_object = WorldObject.from_world_body(req.body) self.unsafe_get_world().robot.attach_urdf_object(world_object, req.pose.header.frame_id, req.pose.pose) logging.loginfo(u'--> attached object {} on link {}'.format(req.body.name, req.pose.header.frame_id)) m = world_object.as_marker_msg() m.pose = req.pose.pose m.header = req.pose.header try: m.frame_locked = True self.publish_object_as_marker(m) except: pass
def attach_object(self, req): """ :type req: UpdateWorldRequest """ if self.get_world().has_object(req.body.name): p = PoseStamped() p.header.frame_id = self.map_frame p.pose = self.get_world().get_object(req.body.name).base_pose p = transform_pose(req.pose.header.frame_id, p) world_object = self.get_world().get_object(req.body.name) self.get_world().attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose) m = world_object.as_marker_msg() m.header.frame_id = p.header.frame_id m.pose = p.pose else: world_object = WorldObject.from_world_body(req.body) self.get_world().robot.attach_urdf_object(world_object, req.pose.header.frame_id, req.pose.pose) m = world_object.as_marker_msg() m.pose = req.pose.pose m.header = req.pose.header try: m.frame_locked = True self.publish_object_as_marker(m) except: pass
def __add_pybullet_bug_fix_hack(self): if not self.has_object(self.hack_name): path = resolve_ros_iris( u'package://giskardpy/urdfs/tiny_ball.urdf') plane = WorldObject.from_urdf_file(path) plane.set_name(self.hack_name) self.add_object(plane)
def update(self): try: while not self.lock.empty(): updates = self.lock.get() for object_state in updates.object_states: # type: ObjectState object_name = object_state.object_id if not self.get_world().has_object(object_name): try: world_object = WorldObject.from_object_state( object_state) except CorruptShapeException: # skip objects without visual continue try: self.get_world().add_object(world_object) except pybullet.error as e: logging.logwarn( u'mesh \'{}\' does not exist'.format( object_state.mesh_path)) continue pose_in_map = transform_pose(MAP, object_state.pose).pose self.get_world().set_object_pose(object_name, pose_in_map) except Empty: pass return Status.RUNNING
def test_god_map_with_world(self): gm = GodMap() w = World() r = WorldObject(pr2_urdf()) w.add_robot(r, PoseStamped(), [], 0, KeyDefaultDict(lambda key: 0), False) gm.safe_set_data([u'world'], w) assert r == gm.safe_get_data([u'world', u'robot'])
def __add_ground_plane(self): """ Adds a ground plane to the Bullet World. """ if not self.has_object(self.ground_plane_name): path = resolve_ros_iris( u'package://giskardpy/urdfs/ground_plane.urdf') plane = WorldObject.from_urdf_file(path) plane.set_name(self.ground_plane_name) self.add_object(plane)
def test_god_map_with_world(self): gm = GodMap() w = World() r = WorldObject(pr2_urdf()) w.add_robot(robot=r, base_pose=PoseStamped(), controlled_joints=[], ignored_pairs=set(), added_pairs=set()) gm.set_data([u'world'], w) gm_robot = gm.get_data(identifier.robot) assert 'pr2' == gm_robot.get_name()
def test_detach_object2(self, test_folder): r = self.cls(donbot_urdf(), path_to_data_folder=test_folder) r.init_self_collision_matrix() scm = r.get_self_collision_matrix() box = WorldObject.from_world_body(make_world_body_box()) p = Pose() p.position = Point(0, 0, 0) p.orientation = Quaternion(0, 0, 0, 1) r.attach_urdf_object(box, u'gripper_tool_frame', p) assert len(scm) < len(r.get_self_collision_matrix()) r.detach_sub_tree(box.get_name()) assert scm.symmetric_difference(r.get_self_collision_matrix()) == set()
def detach(self, joint_name, from_obj=None): if from_obj is None or self.robot.get_name() == from_obj: # this only works because attached simple objects have joint names equal to their name p = self.robot.get_fk_pose(self.robot.get_root(), joint_name) p_map = kdl_to_pose(self.robot.root_T_map.Inverse() * msg_to_kdl(p)) cut_off_obj = self.robot.detach_sub_tree(joint_name) else: raise UnsupportedOptionException( u'only detach from robot supported') wo = WorldObject.from_urdf_object(cut_off_obj) # type: WorldObject wo.base_pose = p_map self.add_object(wo)
def detach(self, joint_name, from_obj=None): if joint_name not in self.robot.get_joint_names(): raise UnknownBodyException(u'can\'t detach: {}'.format(joint_name)) if from_obj is None or self.robot.get_name() == from_obj: # this only works because attached simple objects have joint names equal to their name p = self.robot.get_fk_pose(self.robot.get_root(), joint_name) p_map = kdl_to_pose(self.robot.root_T_map.Inverse() * msg_to_kdl(p)) parent_link = self.robot.get_parent_link_of_joint(joint_name) cut_off_obj = self.robot.detach_sub_tree(joint_name) logging.loginfo(u'<-- detached {} from link {}'.format(joint_name, parent_link)) else: raise UnsupportedOptionException(u'only detach from robot supported') wo = WorldObject.from_urdf_object(cut_off_obj) # type: WorldObject wo.base_pose = p_map self.add_object(wo)
def test_god_map_with_world(self): gm = GodMap() w = World() r = WorldObject(pr2_urdf()) w.add_robot(robot=r, base_pose=PoseStamped(), controlled_joints=[], joint_vel_limit=KeyDefaultDict(lambda key: 0), joint_acc_limit=KeyDefaultDict(lambda key: 0), joint_weights=KeyDefaultDict(lambda key: 0), calc_self_collision_matrix=False, ignored_pairs=set(), added_pairs=set(), symengine_backend='llvm', symengine_opt_level=0) gm.safe_set_data([u'world'], w) gm_robot = gm.safe_get_data(identifier.robot) assert 'pr2' == gm_robot.get_name()
def add_object(self, req): """ :type req: UpdateWorldRequest """ world_body = req.body global_pose = transform_pose(self.map_frame, req.pose).pose world_object = WorldObject.from_world_body(world_body) self.get_world().add_object(world_object) self.get_world().set_object_pose(world_body.name, global_pose) try: m = self.get_world().get_object(world_body.name).as_marker_msg() m.header.frame_id = self.map_frame self.publish_object_as_marker(m) except: pass # SUB-CASE: If it is an articulated object, open up a joint state subscriber # FIXME also keep track of base pose if world_body.joint_state_topic: callback = (lambda msg: self.object_js_cb(world_body.name, msg)) self.object_js_subs[world_body.name] = \ rospy.Subscriber(world_body.joint_state_topic, JointState, callback, queue_size=1)
def initialize_god_map(): god_map = GodMap() blackboard = Blackboard blackboard.god_map = god_map god_map.safe_set_data(identifier.rosparam, rospy.get_param(rospy.get_name())) god_map.safe_set_data(identifier.robot_description, rospy.get_param(u'robot_description')) path_to_data_folder = god_map.safe_get_data(identifier.data_folder) # fix path to data folder if not path_to_data_folder.endswith(u'/'): path_to_data_folder += u'/' god_map.safe_set_data(identifier.data_folder, path_to_data_folder) # fix nWSR nWSR = god_map.safe_get_data(identifier.nWSR) if nWSR == u'None': nWSR = None god_map.safe_set_data(identifier.nWSR, nWSR) pbw.start_pybullet(god_map.safe_get_data(identifier.gui)) while not rospy.is_shutdown(): try: controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state', JointTrajectoryControllerState, timeout=5.0).joint_names except ROSException as e: logging.logerr(u'state topic not available') logging.logerr(e) else: break rospy.sleep(0.5) joint_weight_symbols = process_joint_specific_params(identifier.joint_weights, identifier.default_joint_weight_identifier, god_map) process_joint_specific_params(identifier.collisions_distances, identifier.default_collision_distances, god_map) joint_vel_symbols = process_joint_specific_params(identifier.joint_vel, identifier.default_joint_vel, god_map) joint_acc_symbols = process_joint_specific_params(identifier.joint_acc, identifier.default_joint_acc, god_map) world = PyBulletWorld(god_map.safe_get_data(identifier.gui), blackboard.god_map.safe_get_data(identifier.data_folder)) god_map.safe_set_data(identifier.world, world) robot = WorldObject(god_map.safe_get_data(identifier.robot_description), None, controlled_joints) world.add_robot(robot, None, controlled_joints, joint_vel_symbols, joint_acc_symbols, joint_weight_symbols, True, ignored_pairs=god_map.safe_get_data(identifier.ignored_self_collisions), added_pairs=god_map.safe_get_data(identifier.added_self_collisions), symengine_backend=god_map.safe_get_data(identifier.symengine_backend), symengine_opt_level=god_map.safe_get_data(identifier.symengine_opt_level)) joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(), identifier.joint_states, suffix=[u'position']) joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(), identifier.joint_states, suffix=[u'velocity']) world.robot.reinitialize(joint_position_symbols.joint_map, joint_vel_symbols.joint_map) return god_map
def initialize_god_map(): god_map = GodMap() blackboard = Blackboard blackboard.god_map = god_map load_config_file() god_map.set_data(identifier.rosparam, rospy.get_param(rospy.get_name())) god_map.set_data(identifier.robot_description, rospy.get_param(u'robot_description')) path_to_data_folder = god_map.get_data(identifier.data_folder) # fix path to data folder if not path_to_data_folder.endswith(u'/'): path_to_data_folder += u'/' god_map.set_data(identifier.data_folder, path_to_data_folder) # fix nWSR nWSR = god_map.get_data(identifier.nWSR) if nWSR == u'None': nWSR = None god_map.set_data(identifier.nWSR, nWSR) pbw.start_pybullet(god_map.get_data(identifier.gui)) while not rospy.is_shutdown(): try: controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state', JointTrajectoryControllerState, timeout=5.0).joint_names except ROSException as e: logging.logerr(u'state topic not available') logging.logerr(str(e)) else: break rospy.sleep(0.5) joint_weight_symbols = process_joint_specific_params(identifier.joint_weight, identifier.joint_weight_default, identifier.joint_weight_override, god_map) process_joint_specific_params(identifier.self_collision_avoidance_distance, identifier.self_collision_avoidance_default_threshold, identifier.self_collision_avoidance_default_override, god_map) process_joint_specific_params(identifier.external_collision_avoidance_distance, identifier.external_collision_avoidance_default_threshold, identifier.external_collision_avoidance_default_override, god_map) # TODO add checks to test if joints listed as linear are actually linear joint_velocity_linear_limit_symbols = process_joint_specific_params(identifier.joint_velocity_linear_limit, identifier.joint_velocity_linear_limit_default, identifier.joint_velocity_linear_limit_override, god_map) joint_velocity_angular_limit_symbols = process_joint_specific_params(identifier.joint_velocity_angular_limit, identifier.joint_velocity_angular_limit_default, identifier.joint_velocity_angular_limit_override, god_map) joint_acceleration_linear_limit_symbols = process_joint_specific_params(identifier.joint_acceleration_linear_limit, identifier.joint_acceleration_linear_limit_default, identifier.joint_acceleration_linear_limit_override, god_map) joint_acceleration_angular_limit_symbols = process_joint_specific_params( identifier.joint_acceleration_angular_limit, identifier.joint_acceleration_angular_limit_default, identifier.joint_acceleration_angular_limit_override, god_map) world = PyBulletWorld(False, blackboard.god_map.get_data(identifier.data_folder)) god_map.set_data(identifier.world, world) robot = WorldObject(god_map.get_data(identifier.robot_description), None, controlled_joints) world.add_robot(robot, None, controlled_joints, ignored_pairs=god_map.get_data(identifier.ignored_self_collisions), added_pairs=god_map.get_data(identifier.added_self_collisions)) joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(), identifier.joint_states, suffix=[u'position']) joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(), identifier.joint_states, suffix=[u'velocity']) world.robot.update_joint_symbols(joint_position_symbols.joint_map, joint_vel_symbols.joint_map, joint_weight_symbols, joint_velocity_linear_limit_symbols, joint_velocity_angular_limit_symbols, joint_acceleration_linear_limit_symbols, joint_acceleration_angular_limit_symbols) world.robot.init_self_collision_matrix() return god_map