def cb_obj_model_changed(self, model): print('URDF model changed') if type(model) is not URDFRobot: raise Exception( 'Path "{}" does not refer to a urdf_robot. Type is "{}"'. format(self.urdf_path, type(model))) self.obj = model self.possible_targets = [(Path(joint.child), joint.position) for jname, joint in self.obj.joints.items() if joint.type in goal_joints] self.target_symbol_map = dict(self.possible_targets) self.obj_world = self.km.get_active_geometry( {s for _, s in self.possible_targets if cm.is_symbol(s)}) new_state = { s: 0.0 for _, s in self.possible_targets if cm.is_symbol(s) } self.obj_js_aliases = { str(Path(erase_type(s))[len(self.urdf_path):]): s for s in new_state.keys() } print('Object aliases:\n{}'.format('\n'.join([ '{:>20}: {}'.format(k, v) for k, v in self.obj_js_aliases.items() ]))) self.inverse_js_aliases.update( {erase_type(v): k for k, v in self.obj_js_aliases.items()}) self.state.update(new_state)
def init(self, path, frame): self.frame = frame attrs = collect_paths(self.frame, Path('frame')) super(CreateRelativeFrame, self).init('Frame', [str(a) for a in attrs], parent=Path(self.frame.parent), **{str(a): path + a[1:] for a in attrs})
def insert_diff_base(km, robot_path, root_link, r_wheel_position, l_wheel_position, world_frame='world', wheel_radius=0.06, wheel_distance=0.4, wheel_vel_limit=0.6): if not km.has_data(world_frame): km.apply_operation_before(f'create {world_frame}', f'create {robot_path}', ExecFunction(Path(world_frame), Frame, '')) base_joint_path = robot_path + Path(f'joints/to_{world_frame}') base_op = ExecFunction(base_joint_path, create_diff_drive_joint_with_symbols, CPath(f'{world_frame}/pose'), CPath(robot_path + CPath(f'links/{root_link}/pose')), wheel_radius, wheel_distance, wheel_vel_limit, r_wheel_position, l_wheel_position, CPath(robot_path)) km.apply_operation_after(f'create {base_joint_path}', f'create {robot_path}/links/{root_link}', base_op) km.apply_operation_after(f'connect {world_frame} {robot_path}/links/{root_link}', f'create {base_joint_path}', CreateAdvancedFrameConnection(base_joint_path, Path(world_frame), robot_path + Path(f'links/{root_link}'))) km.clean_structure()
def from_dict(cls, init_dict): km = GeometryModel() urdf = urdf_filler(URDF.from_xml_string(rospy.get_param('/robot_description'))) load_urdf(km, Path(urdf.name), urdf) km.clean_structure() km.dispatch_events() base_frame = init_dict['reference_frame'] eefs = [Endeffector.from_dict(km.get_data(Path(urdf.name)), base_frame, d) for d in init_dict['links']] return cls(km, Path(urdf.name), eefs)
def register_on_model_changed(self, path, callback): if type(path) == str: path = Path(path) self.km.register_on_model_changed(path, callback) if path not in self.tracked_paths: self._start_tracking(path)
def load_model(km, model_path, reference_frame, x, y, z, yaw): origin_pose = gm.frame3_rpy(0, 0, yaw, gm.point3(x, y, z)) if model_path != 'nobilia': urdf_model = load_urdf_file(model_path) load_urdf(km, Path(urdf_model.name), urdf_model, reference_frame, root_transform=origin_pose) return urdf_model.name else: create_nobilia_shelf(km, Path('nobilia'), origin_pose, parent_path=Path(reference_frame)) return 'nobilia'
def get_data(self, path): if type(path) == str: path = Path(path) if path not in self.tracked_paths: self._start_tracking(path) return self.km.get_data(path)
def init(self, transform_path, from_frame, to_frame): self.tf_obj = Transform(str(from_frame), str(to_frame), None) attrs = collect_paths(self.tf_obj, Path('transform')) super(CreateRelativeTransform, self).init('Relative Transform', [str(a) for a in attrs], frame_a=from_frame, frame_b=to_frame, **{str(a): transform_path + a[1:] for a in attrs})
def insert_omni_base(km, robot_path, root_link, world_frame='world', lin_vel=1.0, ang_vel=0.6): if not km.has_data(world_frame): km.apply_operation_before(f'create {world_frame}', f'create {robot_path}', ExecFunction(Path(world_frame), Frame, '')) base_joint_path = robot_path + Path(f'joints/to_{world_frame}') base_op = ExecFunction(base_joint_path, create_omnibase_joint_with_symbols, CPath(f'{world_frame}/pose'), CPath(robot_path + Path(f'links/{root_link}/pose')), gm.vector3(0,0,1), 1.0, 0.6, CPath(robot_path)) km.apply_operation_after(f'create {base_joint_path}', f'create {robot_path}/links/{root_link}', base_op) km.apply_operation_after(f'connect {world_frame} {robot_path}/links/{root_link}', f'create {base_joint_path}', CreateAdvancedFrameConnection(base_joint_path, Path(world_frame), robot_path + Path(f'links/{root_link}'))) km.clean_structure()
def get_root_frames(frame_dict): if len(frame_dict) > 0: if type(frame_dict.keys()[0]) == str: return { k: f for k, f in frame_dict.items() if f.parent not in frame_dict } return { k: f for k, f in frame_dict.items() if Path(f.parent) not in frame_dict } return {}
def track(self, model_path, alias): with self.lock: if type(model_path) is not str: model_path = str(model_path) start_tick = len(self.tracked_poses) == 0 and self._use_timer if model_path not in self.tracked_poses: syms = [ Position(Path(model_path) + (x, )) for x in ['x', 'y', 'z', 'qx', 'qy', 'qz', 'qw'] ] def rf(msg, state): state[syms[0]] = msg.position.x state[syms[1]] = msg.position.y state[syms[2]] = msg.position.z state[syms[3]] = msg.orientation.x state[syms[4]] = msg.orientation.y state[syms[5]] = msg.orientation.z state[syms[6]] = msg.orientation.w def process_model_update(data): self._generate_pose_constraints(model_path, data) axis = vector3(*syms[:3]) self.aliases[alias] = model_path self.tracked_poses[model_path] = TrackerEntry( frame3_quaternion(*syms), rf, process_model_update) self._unintialized_poses.add(model_path) if type(self.km_client) == ModelClient: self.km_client.register_on_model_changed( Path(model_path), process_model_update) else: process_model_update(self.km_client.get_data(model_path)) if start_tick: self.timer = rospy.Timer(rospy.Duration(1.0 / 50), self.cb_tick)
def register_on_model_changed(self, path, callback): if type(path) is str: path = Path(path) if self.__in_dispatch_mode: self.__callback_additions.append((path, callback)) else: if path not in self.model_change_callbacks: self.model_change_callbacks[path] = set() self.model_change_callbacks[path].add(callback) if callback not in self._callback_path_registry: self._callback_path_registry[callback] = set() self._callback_path_registry[callback].add(path)
def save_to_file(self, f, subpath=Path('')): self.clean_structure() if len(subpath) != 0: json.dump([{ 'op': top.op, 'tag': top.tag } for top in self.get_history_of(subpath)], f) else: json.dump([{ 'op': chunk.operation, 'tag': t } for (_, t), chunk in zip( sorted([(s, t) for t, s in self.timeline_tags.items()]), self.operation_history.chunk_history)], f)
def _dispatch_model_events(pdict, data, key, call_tracker=set()): for cb in pdict.value: if cb not in call_tracker: cb(data) call_tracker.add(cb) if len(key) == 0: for k, p in pdict.items(): _dispatch_model_events(p, Path(k).get_data_no_throw(data), key, call_tracker) else: _dispatch_model_events(pdict.get_sub_dict(key[:1]), key[:1].get_data_no_throw(data), key[1:], call_tracker)
def _update_model(self, msg): if len(msg.paths) != len(msg.data): raise Exception('Message path and data arrays need to be of equal length.\n Paths: {}\n Data: {}'.format(len(msg.paths), len(msg.data))) for str_path, data in zip(msg.paths, msg.data): path = Path(str_path) if path in self.tracked_paths: print('Updating {}'.format(path)) self.km.set_data(path, json.loads(data)) for cmsg in msg.constraints: c = decode_constraint_filtered(cmsg, self.tracked_symbols) if c is not None: self.km.add_constraint(cmsg.name, c) elif self.km.has_constraint(cmsg.name): self.km.remove_constraint(cmsg.name)
def dispatch_events(self): static_objects = [] static_poses = [] dynamic_poses = {} for str_k in self._collision_objects: k = Path(str_k) if k in self._callback_batch: #print('{} is a collision link and has changed in the last update batch'.format(k)) if self.has_data(k): #print('{} was changed'.format(k)) link = self.get_data(k) pose_expr = link.pose # * link.collision.to_parent symbol_set = set() if type(pose_expr) is GM: symbol_set = symbol_set.union(pose_expr.diff_symbols) pose_expr = pose_expr.to_sym_matrix() symbol_set |= pose_expr.free_symbols.union( {get_diff_symbol(s) for s in pose_expr.free_symbols}) self._co_pose_expr[str_k] = pose_expr self._co_symbol_map[str_k] = symbol_set if len(symbol_set) > 0: for s in symbol_set: if s not in self._symbol_co_map: self._symbol_co_map[s] = set() self._symbol_co_map[s].add(str_k) dynamic_poses[str_k] = np.array( pose_expr.subs( {s: 0 for s in pose_expr.free_symbols}).tolist()) else: #print('{} is static, setting its pose to:\n{}'.format(k, pose_expr)) static_objects.append(self._collision_objects[str_k]) static_poses.append(np.array(pose_expr.tolist())) else: self._process_link_removal(k) if len(static_objects) > 0: pb.batch_set_transforms(static_objects, np.vstack(static_poses)) self._static_objects = static_objects # print('\n '.join(['{}: {}'.format(n, c.transform) for n, c in self._collision_objects.items()])) if len(dynamic_poses) > 0: objs, matrices = zip(*[(self._collision_objects[k], m) for k, m in dynamic_poses.items()]) pb.batch_set_transforms(objs, np.vstack(matrices)) super(GeometryModel, self).dispatch_events()
def __init__(self, robot_prefix, joint_topic, joint_vel_symbols, base_topic=None, base_symbols=None, base_watchdog=rospy.Duration(0.4), reference_frame='odom'): self.control_alias = { s: str(Path(gm.erase_type(s))[-1]) for s in joint_vel_symbols } self._robot_cmd_msg = JointStateMsg() self._robot_cmd_msg.name = list(self.control_alias.keys()) self._state_cb = None self.robot_prefix = robot_prefix if base_symbols is not None: self.base_aliases = base_symbols self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) self.reference_frame = reference_frame self._base_cmd_msg = TwistMsg() self.pub_base_command = rospy.Publisher(base_topic, TwistMsg, queue_size=1, tcp_nodelay=True) self._base_last_cmd_stamp = None self._base_cmd_lock = RLock() self._base_thread = threading.Thread( target=self._base_cmd_repeater, args=(base_watchdog, )) self._base_thread.start() else: self.base_aliases = None self.pub_joint_command = rospy.Publisher(joint_topic, JointStateMsg, queue_size=1, tcp_nodelay=True) self.sub_robot_js = rospy.Subscriber('/joint_states', JointStateMsg, callback=self.cb_robot_js, queue_size=1)
def set_data(self, key, value): if type(key) is str: key = Path(key) if isinstance(value, RigidBody): self._process_body_insertion(key, value) elif isinstance(value, KinematicJoint): self._process_joint_insertion(key, value) elif isinstance(value, ArticulatedObject): for lname, link in value.links.items(): self._process_body_insertion(key + ( 'links', lname, ), link) for jname, joint in value.joints.items(): self._process_joint_insertion(key + ( 'joints', jname, ), joint) super(GeometryModel, self).set_data(key, value)
def get_data(self, key): if type(key) == str: key = Path(key) return self.data_tree[key]
def behavior_update(self): state_count = 0 while not rospy.is_shutdown() and not self._kys: loop_start = rospy.Time.now() with self._state_lock: if self._robot_state_update_count <= state_count: rospy.sleep(0.01) continue state_count = self._robot_state_update_count if self.controller is not None: now = rospy.Time.now() with self._state_lock: deltaT = 0.05 if self._last_controller_update is None else ( now - self._last_controller_update).to_sec() try: command = self.controller.get_cmd(self._state, deltaT=deltaT) except Exception as e: print(traceback.format_exc()) rospy.signal_shutdown('die lol') self.robot_command_processor.send_command(command) self._last_controller_update = now # Lets not confuse the tracker if self._phase != 'opening' and self._last_external_cmd_msg is not None: # Ensure that velocities are assumed to be 0 when not operating anything self._last_external_cmd_msg.value = [0] * len( self._last_external_cmd_msg.value) self.pub_external_command.publish(self._last_external_cmd_msg) self._last_external_cmd_msg = None if self._phase == 'idle': # if there is no controller, instantiate idle # check for new target # -> open gripper # instantiate 6d controller # switch to "grasping" if self.controller is None: self.gripper_wrapper.sync_set_gripper_position(0.07) self.controller = self._idle_controller with self._state_lock: for s, p in self._target_map.items(): if s in self._state and self._state[ s] < self._var_upper_bound[ s] * self.monitoring_threshold: # Some thing in the scene is closed self._current_target = p print(f'New target is {self._current_target}') self.gripper_wrapper.sync_set_gripper_position( 0.07) self._last_controller_update = None print('Gripper is open. Proceeding to grasp...') draw_fn = None eef_pose = self.km.get_data(self.eef_path).pose if self.visualizer is not None: self.visualizer.begin_draw_cycle('grasp pose') self.visualizer.draw_poses( 'grasp pose', np.eye(4), 0.2, 0.01, [ gm.subs(self._grasp_poses[p], self._state) ]) self.visualizer.render('grasp pose') def draw_fn(state): self.visualizer.begin_draw_cycle( 'debug_grasp') self.visualizer.draw_poses( 'debug_grasp', np.eye(4), 1.0, 0.01, [ gm.subs(eef_pose, state), gm.subs(self._grasp_poses[p], state) ]) self.visualizer.render('debug_grasp') # print('\n'.join(f'{s}: {v}' for s, v in state.items())) self.controller = SixDPoseController( self.km, eef_pose, self._grasp_poses[p], self.controlled_symbols, self.weights, draw_fn) self._phase = 'grasping' print(f'Now entering {self._phase} state') break else: print( 'None of the monitored symbols are closed:\n {}'. format('\n '.join( f'{self._state[s]} < {self._var_upper_bound[s]}' for s in self._target_map.keys() if s in self._state))) elif self._phase == 'grasping': # check if grasp is acheived # -> close gripper # instantiate cascading controller # switch to "opening" # if there is no more command but the goal error is too great -> "homing" if self.controller.equilibrium_reached(0.03, -0.03): if self.controller.current_error() > 0.01: self.controller = self._idle_controller self._phase = 'homing' print(f'Now entering {self._phase} state') else: print('Closing gripper...') self.gripper_wrapper.sync_set_gripper_position(0, 80) print('Generating opening controller') eef = self.km.get_data(self.eef_path) obj = self.km.get_data(self._current_target) with self._state_lock: static_eef_pose = gm.subs(eef.pose, self._state) static_object_pose = gm.subs(obj.pose, self._state) offset_pose = np_inverse_frame(static_object_pose).dot( static_eef_pose) print(offset_pose) goal_pose = gm.dot(obj.pose, gm.Matrix(offset_pose)) goal_pos_error = gm.norm( gm.pos_of(eef.pose) - gm.pos_of(goal_pose)) goal_rot_error = gm.norm(eef.pose[:3, :3] - goal_pose[:3, :3]) target_symbols = self._target_body_map[ self._current_target] lead_goal_constraints = { f'open_{s}': SC(self._var_upper_bound[s] - s, 1000, 1, s) for s in target_symbols if s in self._var_upper_bound } for n, c in lead_goal_constraints.items(): print(f'{n}: {c}') follower_goal_constraints = { 'keep position': SC(-goal_pos_error, -goal_pos_error, 10, goal_pos_error), 'keep rotation': SC(-goal_rot_error, -goal_rot_error, 1, goal_rot_error) } blacklist = { gm.Velocity(Path('pr2/torso_lift_joint')) }.union({ gm.DiffSymbol(s) for s in gm.free_symbols(obj.pose) if 'location' in str(s) }) # blacklist = {gm.DiffSymbol(s) for s in gm.free_symbols(obj.pose) if 'location' in str(s)} self.controller = CascadingQP( self.km, lead_goal_constraints, follower_goal_constraints, f_gen_follower_cvs=gen_dv_cvs, controls_blacklist=blacklist, t_follower=GQPB, visualizer=None # self.visualizer ) print(self.controller) # def debug_draw(vis, state, cmd): # vis.begin_draw_cycle('lol') # vis.draw_poses('lol', np.eye(4), 0.2, 0.01, # [gm.subs(goal_pose, state), # gm.subs(eef.pose, state)]) # vis.render('lol') # self.controller.follower_qp._cb_draw = debug_draw # self.gripper_wrapper.sync_set_gripper_position(0.07) # rospy.signal_shutdown('die lol') # return self._last_controller_update = None self._phase = 'opening' print(f'Now entering {self._phase} state') elif self._phase == 'opening': # Wait for monitored symbols to be in open position # -> open gripper # generate 6d retraction goal: -10cm in tool frame # spawn 6d controller # switch to "retracting" # self.visualizer.begin_draw_cycle('world state') # with self._state_lock: # self._world.update_world(self._state) # self.visualizer.draw_world('world state', self._world, b=0) # self.visualizer.render('world state') external_command = { s: v for s, v in command.items() if s not in self.controlled_symbols } ext_msg = ValueMapMsg() ext_msg.header.stamp = now ext_msg.symbol, ext_msg.value = zip( *[(str(s), v) for s, v in external_command.items()]) self.pub_external_command.publish(ext_msg) self._last_external_cmd_msg = ext_msg with self._state_lock: current_external_error = { s: self._state[s] for s in self._target_body_map[self._current_target] } print('Current error state:\n {}'.format('\n '.join( [f'{s}: {v}' for s, v in current_external_error.items()]))) gripper_err = self.gripper_wrapper.get_latest_error() # if gripper_err < 0.0005: # Grasped object is thinner than 5mm aka we lost it # self.controller = self._idle_controller # self._phase = 'homing' # print(f'Grasped object seems to have slipped ({gripper_err}). Returning to home...') # return if min([ v >= self._var_upper_bound[s] * self.acceptance_threshold for s, v in current_external_error.items() ]): print('Target fulfilled. Setting it to None') self._current_target = None eef = self.km.get_data(self.eef_path) with self._state_lock: static_eef_pose = gm.subs(eef.pose, self._state) goal_pose = static_eef_pose.dot( np.array([[1, 0, 0, -0.12], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])) self.controller = SixDPoseController( self.km, eef.pose, goal_pose, self.controlled_symbols, self.weights) self.gripper_wrapper.sync_set_gripper_position(0.07) self._last_controller_update = None self._phase = 'retracting' print(f'Now entering {self._phase} state') else: remainder = (1 / 3) - (rospy.Time.now() - loop_start).to_sec() if remainder > 0: rospy.sleep(remainder) elif self._phase == 'retracting': # Wait for retraction to complete (Currently just skipping) # -> spawn idle controller # switch to "homing" if self.controller.equilibrium_reached(0.035, -0.035): self.controller = self._idle_controller self._phase = 'homing' print(f'Now entering {self._phase} state') elif self._phase == 'homing': # Wait for idle controller to have somewhat low error # -> switch to "idle" if self.controller is None: self.gripper_wrapper.sync_set_gripper_position(0.07) self.controller = self._idle_controller continue if self.controller.equilibrium_reached(0.1, -0.1): self._phase = 'idle' print(f'Now entering {self._phase} state') else: raise Exception(f'Unknown state "{self._phase}')
'PR2 will be loaded from parameter server. It is currently not there.' ) exit(1) urdf_model = load_urdf_file( 'package://iai_pr2_description/robots/pr2_calibrated_with_ft2.xml') # urdf_model = load_urdf_str(rospy.get_param('/robot_description')) if urdf_model.name.lower() != 'pr2': print( f'The loaded robot is not the PR2. Its name is "{urdf_model.name}"' ) exit(1) km = GeometryModel() load_urdf(km, Path('pr2'), urdf_model) km.clean_structure() reference_frame = rospy.get_param('~reference_frame', urdf_model.get_root()) use_base = reference_frame != urdf_model.get_root() if use_base: insert_omni_base(km, Path('pr2'), urdf_model.get_root(), reference_frame) base_joint_path = Path(f'pr2/joints/to_{reference_frame}') visualizer = ROSBPBVisualizer('~vis', base_frame=reference_frame) model_name = load_localized_model(km, model_path, reference_frame)
res.objects = fake_observation() return res if __name__ == '__main__': if len(sys.argv) < 2: print('SDF file needed.') exit(0) kin_links = None for a in sys.argv[1:]: if not ':=' in a: world_path = res_pkg_path(a) print('Loading world file {}'.format(world_path)) sdf = SDF(load_xml(world_path)) kin_links = world_to_links(sdf.worlds.values()[0], Path('bla'), False) break if kin_links is None: print('No SDF file was given.') exit(0) rospy.init_node('fake_observation') robot_name = rospy.get_param('~robot_name', 'fetch') width = rospy.get_param('~width', 640) height = rospy.get_param('~height', 480) near = rospy.get_param('~near', 0.2) far = rospy.get_param('~far', 7.0) fov = rospy.get_param('~fov', 60) * (np.pi / 180) right = np.tan(0.5 * fov) * near
def create_nobilia_shelf(km, prefix, origin_pose=gm.eye(4), parent_path=Path('world')): km.apply_operation( f'create {prefix}', ExecFunction(prefix, MarkedArticulatedObject, str(prefix))) shelf_height = 0.72 shelf_width = 0.6 shelf_body_depth = 0.35 wall_width = 0.016 l_prefix = prefix + ('links', ) geom_body_wall_l = Box( l_prefix + ('body', ), gm.translation3(0, 0.5 * (shelf_width - wall_width), 0), gm.vector3(shelf_body_depth, wall_width, shelf_height)) geom_body_wall_r = Box( l_prefix + ('body', ), gm.translation3(0, -0.5 * (shelf_width - wall_width), 0), gm.vector3(shelf_body_depth, wall_width, shelf_height)) geom_body_ceiling = Box( l_prefix + ('body', ), gm.translation3(0, 0, 0.5 * (shelf_height - wall_width)), gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width)) geom_body_floor = Box( l_prefix + ('body', ), gm.translation3(0, 0, -0.5 * (shelf_height - wall_width)), gm.vector3(shelf_body_depth, shelf_width - wall_width, wall_width)) geom_body_shelf_1 = Box( l_prefix + ('body', ), gm.translation3(0.02, 0, -0.2 * (shelf_height - wall_width)), gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width, wall_width)) geom_body_shelf_2 = Box( l_prefix + ('body', ), gm.translation3(0.02, 0, 0.2 * (shelf_height - wall_width)), gm.vector3(shelf_body_depth - 0.04, shelf_width - wall_width, wall_width)) geom_body_back = Box( l_prefix + ('body', ), gm.translation3(0.5 * (shelf_body_depth - 0.005), 0, 0), gm.vector3(0.005, shelf_width - 2 * wall_width, shelf_height - 2 * wall_width)) shelf_geom = [ geom_body_wall_l, geom_body_wall_r, geom_body_ceiling, geom_body_floor, geom_body_back, geom_body_shelf_1, geom_body_shelf_2 ] rb_body = RigidBody(parent_path, origin_pose, geometry=dict(enumerate(shelf_geom)), collision=dict(enumerate(shelf_geom))) geom_panel_top = Box(l_prefix + ('panel_top', ), gm.eye(4), gm.vector3(0.357, 0.595, wall_width)) geom_panel_bottom = Box(l_prefix + ('panel_bottom', ), gm.eye(4), gm.vector3(0.357, 0.595, wall_width)) handle_width = 0.16 handle_depth = 0.05 handle_diameter = 0.012 geom_handle_r = Box( l_prefix + ('handle', ), gm.translation3(0.5 * handle_depth, 0.5 * (handle_width - handle_diameter), 0), gm.vector3(handle_depth, handle_diameter, handle_diameter)) geom_handle_l = Box( l_prefix + ('handle', ), gm.translation3(0.5 * handle_depth, -0.5 * (handle_width - handle_diameter), 0), gm.vector3(handle_depth, handle_diameter, handle_diameter)) geom_handle_bar = Box( l_prefix + ('handle', ), gm.translation3(handle_depth - 0.5 * handle_diameter, 0, 0), gm.vector3(handle_diameter, handle_width - handle_diameter, handle_diameter)) handle_geom = [geom_handle_l, geom_handle_r, geom_handle_bar] # Sketch of mechanism # # T ---- a # ---- \ Z # b ..... V \ # | ...... d # B | ------ # c ------ # L # # Diagonal V is virtual # # # Angles: # a -> alpha (given) # b -> gamma_1 + gamma_2 = gamma # c -> don't care # d -> delta_1 + delta_2 = delta # opening_position = gm.Position(prefix + ('door', )) # Calibration results # # Solution top hinge: cost = 0.03709980624159568 [ 0.08762252 -0.01433833 0.2858676 0.00871125] # Solution bottom hinge: cost = 0.025004236048128934 [ 0.1072496 -0.01232362 0.27271013 0.00489996] # Added 180 deg rotation due to -x being the forward facing side in this model top_hinge_in_body_marker = gm.translation3(0.08762252 - 0.015, 0, -0.01433833) top_panel_marker_in_top_hinge = gm.translation3(0.2858676 - 0.003, -wall_width + 0.0025, 0.00871125 - 0.003) front_hinge_in_top_panel_maker = gm.translation3(0.1072496 - 0.02, 0, -0.01232362 + 0.007) bottom_panel_marker_in_front_hinge = gm.translation3( 0.27271013, 0, 0.00489996) # Top hinge - Data taken from observation body_marker_in_body = gm.dot( gm.rotation3_axis_angle(gm.vector3(0, 0, 1), math.pi), gm.translation3(0.5 * shelf_body_depth - 0.062, -0.5 * shelf_width + 0.078, 0.5 * shelf_height)) top_panel_marker_in_top_panel = gm.translation3( geom_panel_top.scale[0] * 0.5 - 0.062, -geom_panel_top.scale[1] * 0.5 + 0.062, geom_panel_top.scale[2] * 0.5) bottom_panel_marker_in_bottom_panel = gm.translation3( geom_panel_bottom.scale[0] * 0.5 - 0.062, -geom_panel_bottom.scale[1] * 0.5 + 0.062, geom_panel_bottom.scale[2] * 0.5) top_hinge_in_body = gm.dot(body_marker_in_body, top_hinge_in_body_marker) top_panel_in_top_hinge = gm.dot( top_panel_marker_in_top_hinge, gm.inverse_frame(top_panel_marker_in_top_panel)) front_hinge_in_top_panel = gm.dot(top_panel_marker_in_top_panel, front_hinge_in_top_panel_maker) bottom_panel_in_front_hinge = gm.dot( bottom_panel_marker_in_front_hinge, gm.inverse_frame(bottom_panel_marker_in_bottom_panel)) # Point a in body reference frame point_a = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(top_hinge_in_body)) point_d = gm.point3(-shelf_body_depth * 0.5 + 0.09, 0, shelf_height * 0.5 - 0.192) # point_d = gm.point3(-shelf_body_depth * 0.5 + gm.Symbol('point_d_x'), 0, shelf_height * 0.5 - gm.Symbol('point_d_z')) # Zero alpha along the vertical axis vec_a_to_d = gm.dot(point_d - point_a) alpha = gm.atan2(vec_a_to_d[0], -vec_a_to_d[2]) + opening_position top_panel_in_body = gm.dot( top_hinge_in_body, # Translation hinge to body frame gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -opening_position + 0.5 * math.pi), # Hinge around y top_panel_in_top_hinge) front_hinge_in_body = gm.dot(top_panel_in_body, front_hinge_in_top_panel) # Point b in top panel reference frame point_b_in_top_hinge = gm.pos_of( gm.dot(gm.diag(1, 0, 1, 1), front_hinge_in_top_panel, top_panel_in_top_hinge)) point_b = gm.dot(gm.diag(1, 0, 1, 1), gm.pos_of(front_hinge_in_body)) # Hinge lift arm in body reference frame point_c_in_bottom_panel = gm.dot( gm.diag(1, 0, 1, 1), bottom_panel_marker_in_bottom_panel, gm.point3(-0.094, -0.034, -0.072), # gm.point3(-gm.Symbol('point_c_x'), -0.034, -gm.Symbol('point_c_z')) ) point_c_in_front_hinge = gm.dot( gm.diag(1, 0, 1, 1), gm.dot(bottom_panel_in_front_hinge, point_c_in_bottom_panel)) length_z = gm.norm(point_a - point_d) vec_a_to_b = point_b - point_a length_t = gm.norm(vec_a_to_b) length_b = gm.norm(point_c_in_front_hinge[:3]) # length_l = gm.Symbol('length_l') # 0.34 length_l = 0.372 vec_b_to_d = point_d - point_b length_v = gm.norm(vec_b_to_d) gamma_1 = inner_triangle_angle(length_t, length_v, length_z) gamma_2 = inner_triangle_angle(length_b, length_v, length_l) top_panel_offset_angle = gm.atan2(point_b_in_top_hinge[2], point_b_in_top_hinge[0]) bottom_offset_angle = gm.atan2(point_c_in_front_hinge[2], point_c_in_front_hinge[0]) gamma = gamma_1 + gamma_2 rb_panel_top = RigidBody(l_prefix + ('body', ), gm.dot(rb_body.pose, top_panel_in_body), top_panel_in_body, geometry={0: geom_panel_top}, collision={0: geom_panel_top}) # old offset: 0.5 * geom_panel_top.scale[2] + 0.03 tf_bottom_panel = gm.dot( front_hinge_in_top_panel, gm.rotation3_axis_angle( gm.vector3(0, 1, 0), math.pi + bottom_offset_angle - top_panel_offset_angle), gm.rotation3_axis_angle(gm.vector3(0, -1, 0), gamma), bottom_panel_in_front_hinge) rb_panel_bottom = RigidBody(l_prefix + ('panel_top', ), gm.dot(rb_panel_top.pose, tf_bottom_panel), tf_bottom_panel, geometry={0: geom_panel_bottom}, collision={0: geom_panel_bottom}) handle_transform = gm.dot( gm.translation3(geom_panel_bottom.scale[0] * 0.5 - 0.08, 0, 0.5 * wall_width), gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -math.pi * 0.5)) rb_handle = RigidBody(l_prefix + ('panel_bottom', ), gm.dot(rb_panel_bottom.pose, handle_transform), handle_transform, geometry={x: g for x, g in enumerate(handle_geom)}, collision={x: g for x, g in enumerate(handle_geom)}) # Only debugging point_c = gm.dot(rb_panel_bottom.pose, point_c_in_bottom_panel) vec_b_to_c = point_c - point_b km.apply_operation(f'create {prefix}/links/body', CreateValue(rb_panel_top.parent, rb_body)) km.apply_operation(f'create {prefix}/links/panel_top', CreateValue(rb_panel_bottom.parent, rb_panel_top)) km.apply_operation( f'create {prefix}/links/panel_bottom', CreateValue(l_prefix + ('panel_bottom', ), rb_panel_bottom)) km.apply_operation(f'create {prefix}/links/handle', CreateValue(l_prefix + ('handle', ), rb_handle)) km.apply_operation( f'create {prefix}/joints/hinge', ExecFunction( prefix + Path('joints/hinge'), RevoluteJoint, CPath(rb_panel_top.parent), CPath(rb_panel_bottom.parent), opening_position, gm.vector3(0, 1, 0), gm.eye(4), 0, 1.84, **{ f'{opening_position}': Constraint(0 - opening_position, 1.84 - opening_position, opening_position), f'{gm.DiffSymbol(opening_position)}': Constraint(-0.25, 0.25, gm.DiffSymbol(opening_position)) })) m_prefix = prefix + ('markers', ) km.apply_operation( f'create {prefix}/markers/body', ExecFunction(m_prefix + ('body', ), Frame, CPath(l_prefix + ('body', )), gm.dot(rb_body.pose, body_marker_in_body), body_marker_in_body)) km.apply_operation( f'create {prefix}/markers/top_panel', ExecFunction(m_prefix + ('top_panel', ), Frame, CPath(l_prefix + ('panel_top', )), gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel), top_panel_marker_in_top_panel)) km.apply_operation( f'create {prefix}/markers/bottom_panel', ExecFunction( m_prefix + ('bottom_panel', ), Frame, CPath(l_prefix + ('panel_bottom', )), gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel), bottom_panel_marker_in_bottom_panel)) return NobiliaDebug( [ top_hinge_in_body, gm.dot( top_hinge_in_body, gm.rotation3_axis_angle(gm.vector3(0, 1, 0), -opening_position + 0.5 * math.pi), top_panel_in_top_hinge, front_hinge_in_top_panel), body_marker_in_body, gm.dot(rb_panel_top.pose, top_panel_marker_in_top_panel), gm.dot(rb_panel_bottom.pose, bottom_panel_marker_in_bottom_panel) ], [(point_a, vec_a_to_d), (point_a, vec_a_to_b), (point_b, vec_b_to_d), (point_b, vec_b_to_c)], { 'gamma_1': gamma_1, 'gamma_1 check_dot': gamma_1 - gm.acos( gm.dot_product(-vec_a_to_b / gm.norm(vec_a_to_b), vec_b_to_d / gm.norm(vec_b_to_d))), 'gamma_1 check_cos': gamma_1 - inner_triangle_angle( gm.norm(vec_a_to_b), gm.norm(vec_b_to_d), gm.norm(vec_a_to_d)), 'gamma_2': gamma_2, 'gamma_2 check_dot': gamma_2 - gm.acos( gm.dot_product(vec_b_to_c / gm.norm(vec_b_to_c), vec_b_to_d / gm.norm(vec_b_to_d))), 'length_v': length_v, 'length_b': length_b, 'length_l': length_l, 'position': opening_position, 'alpha': alpha, 'dist c d': gm.norm(point_d - point_c) }, { gm.Symbol('point_c_x'): 0.094, gm.Symbol('point_c_z'): 0.072, gm.Symbol('point_d_x'): 0.09, gm.Symbol('point_d_z'): 0.192, gm.Symbol('length_l'): 0.372 })
robot_str = [ x for x in sys.argv[1:] if ':=' not in x ][0] if len([x for x in sys.argv[1:] if ':=' not in x]) > 0 else 'fetch' if robot_str == 'fetch': resting_pose = { "shoulder_pan_joint": 1.32, "shoulder_lift_joint": 1.40, "upperarm_roll_joint": -0.2, "elbow_flex_joint": 1.72, "forearm_roll_joint": 0.0, "wrist_flex_joint": 1.66, "wrist_roll_joint": 0.0 } closer = ObsessiveObjectCloser(Path('iai_oven_area'), Path('fetch/links/gripper_link'), Path('fetch/links/head_camera_link'), '/tracked/state', '/fetch', resting_pose=resting_pose) elif robot_str == 'pr2': resting_pose = { 'l_elbow_flex_joint': -2.1213, 'l_shoulder_lift_joint': 1.2963, 'l_shoulder_pan_joint': 0.0, 'l_upper_arm_roll_joint': 0.0, 'l_forearm_roll_joint': 0.0, 'l_wrist_flex_joint': -1.05, 'r_elbow_flex_joint': -2.1213, 'r_shoulder_lift_joint': 1.2963,
kitchen_model = urdf_filler( URDF.from_xml_string(hacky_urdf_parser_fix(urdf_kitchen_str))) # traj_pup = rospy.Publisher('/{}/commands/joint_trajectory'.format( urdf_model.name), JointTrajectoryMsg, queue_size=1) kitchen_traj_pup = rospy.Publisher('/{}/commands/joint_trajectory'.format( kitchen_model.name), JointTrajectoryMsg, queue_size=1) # KINEMATIC MODEL km = GeometryModel() load_urdf(km, Path(robot), urdf_model) load_urdf(km, Path('kitchen'), kitchen_model) # create_nobilia_shelf(km, Path('nobilia'), gm.frame3_rpy(0, 0, 0.4, # gm.point3(1.2, 0, 0.8))) km.clean_structure() km.apply_operation_before('create world', 'create {}'.format(robot), ExecFunction(Path('world'), Frame, '')) base_joint_path = Path(f'{robot}/joints/to_world') # Insert base to world kinematic if use_omni: insert_omni_base(km, Path(robot), urdf_model.get_root()) else: insert_diff_base(km,
def create_door(km, prefix, height, width, frame_width=0.05, to_world_tf=gm.eye(4)): km.apply_operation(f'create {prefix}', ExecFunction(prefix, ArticulatedObject, 'door')) prefix = prefix + ('links', ) base_plate_geom = Box(prefix + ('frame', ), gm.translation3(0, 0, 0.015), gm.vector3(0.2, width + 0.2, 0.03)) frame_pillar_l_geom = Box( prefix + ('frame', ), gm.translation3(0, 0.5 * (width + frame_width), 0.5 * height + 0.03), gm.vector3(frame_width, frame_width, height)) frame_pillar_r_geom = Box( prefix + ('frame', ), gm.translation3(0, -0.5 * (width + frame_width), 0.5 * height + 0.03), gm.vector3(frame_width, frame_width, height)) frame_bar_geom = Box( prefix + ('frame', ), gm.translation3(0, 0, height + 0.5 * frame_width + 0.03), gm.vector3(frame_width, width + 2 * frame_width, frame_width)) frame_rb = RigidBody(Path('world'), to_world_tf, geometry={ 1: base_plate_geom, 2: frame_pillar_l_geom, 3: frame_pillar_r_geom, 4: frame_bar_geom }, collision={ 1: base_plate_geom, 2: frame_pillar_l_geom, 3: frame_pillar_r_geom, 4: frame_bar_geom }) door_geom1 = Box(prefix + ('door', ), gm.translation3(0.015, 0, 0), gm.vector3(0.03, width, height)) door_geom2 = Box(prefix + ('door', ), gm.translation3(-0.005, 0, 0.01), gm.vector3(0.01, width + 0.02, height + 0.01)) handle_bar_geom = Box(prefix + ('handle', ), gm.translation3(-0.08, 0.06, 0), gm.vector3(0.02, 0.12, 0.02)) handle_cylinder_geom = Cylinder( prefix + ('handle', ), gm.dot(gm.translation3(-0.04, 0, 0), gm.rotation3_axis_angle(gm.vector3(0, 1, 0), 0.5 * math.pi)), 0.02, 0.08) door_rb = RigidBody(Path(f'{prefix}/frame'), gm.translation3(0.0, 0.5 * -width - 0.01, 0), geometry={ 1: door_geom1, 2: door_geom2 }, collision={ 1: door_geom1, 2: door_geom2 }) handle_rb = RigidBody(Path(f'{prefix}/door'), gm.eye(4), geometry={ 1: handle_bar_geom, 2: handle_cylinder_geom }, collision={ 1: handle_bar_geom, 2: handle_cylinder_geom }) km.apply_operation(f'create {prefix}/frame', CreateValue(prefix + ('frame', ), frame_rb)) km.apply_operation(f'create {prefix}/door', CreateValue(prefix + ('door', ), door_rb)) km.apply_operation(f'create {prefix}/handle', CreateValue(prefix + ('handle', ), handle_rb)) door_position = gm.Position('door') handle_position = gm.Position('handle') prefix = prefix[:-1] + ('joints', ) km.apply_operation( f'create {prefix}', ExecFunction( prefix + ('hinge', ), RevoluteJoint, CPath(door_rb.parent), CPath(handle_rb.parent), door_position, gm.vector3(0, 0, -1), gm.translation3(0.5 * -frame_width - 0.005, 0.5 * width + 0.01, 0.5 * height + 0.03), 0, 0.75 * math.pi, 100, 1, 0)) km.apply_operation( f'create {prefix}', ExecFunction(prefix + ('handle', ), RevoluteJoint, CPath(handle_rb.parent), CPath(f'{prefix}/handle'), handle_position, gm.vector3(-1, 0, 0), gm.translation3(0, -0.5 * width - 0.02 + 0.06, 0), 0, 0.25 * math.pi, 100, 1, 0)) prefix = prefix[:-1] km.apply_operation( f'connect {prefix}/links/frame {prefix}/links/door', CreateURDFFrameConnection(prefix + ('joints', 'hinge'), Path(door_rb.parent), Path(handle_rb.parent))) km.apply_operation( f'connect {prefix}/links/door {prefix}/links/handle', CreateURDFFrameConnection(prefix + ('joints', 'handle'), Path(handle_rb.parent), Path(f'{prefix}/links/handle'))) km.apply_operation( f'add lock {door_position}', ConditionalDoorHandleConstraints(door_position, handle_position, math.pi * 0.01, math.pi * 0.15))
def create_symbol_vec3(prefix): if not isinstance(prefix, Path): prefix = Path(prefix) symbols = [p.to_symbol() for p in [prefix + (x,) for x in 'xyz']] return SymbolVector3(gm.vector3(*symbols), *symbols)
def remove_data(self, key): if type(key) == str: key = Path(key) self.data_tree.remove_data(key)
def create_symbol_frame3_rpy(prefix): if not isinstance(prefix, Path): prefix = Path(prefix) symbols = [p.to_symbol() for p in ([prefix + (x,) for x in 'xyz'] + [prefix + (f'r{r}',) for r in 'rpy'])] return SymbolPose3RPY(gm.frame3_rpy(*symbols[-3:], gm.point3(*symbols[:3])), *symbols)
def set_data(self, key, value): if type(key) == str: key = Path(key) self.data_tree[key] = value