def test_model_reform(self): ks = ArticulationModel() a = Position('a') b = Position('b') c = Position('c') parent_pose_a = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5)) parent_pose_b = frame3_axis_angle(vector3(1,0,0), b, point3(7* b, 0, 5)) joint_transform = translation3(7, -5, 33) axis = vector3(1, -3, 7) axis = axis position = c child_pose_a = parent_pose_a * joint_transform * translation3(*(axis[:,:3] * position)) child_pose_b = parent_pose_b * joint_transform * translation3(*(axis[:,:3] * position)) ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose_a))) ks.apply_operation('create child', CreateComplexObject(Path('child'), KinematicLink('', se.eye(4)))) self.assertTrue(ks.has_data('parent/pose')) self.assertTrue(ks.has_data('child/pose')) ks.apply_operation('connect parent child', SetPrismaticJoint(Path('parent/pose'), Path('child/pose'), Path('fixed_joint'), joint_transform, axis, position, -1, 2, 0.5)) self.assertTrue(ks.has_data('fixed_joint')) self.assertEquals(ks.get_data('child/pose'), child_pose_a) ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose_b))) ks.clean_structure() self.assertTrue(ks.has_data('fixed_joint')) self.assertEquals(ks.get_data('child/pose'), child_pose_b)
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 __init__(self, km, controlled_symbols, resting_pose, camera_path=None): tucking_constraints = {} if resting_pose is not None: tucking_constraints = { f'tuck {s}': SC(p - s, p - s, 1, s) for s, p in resting_pose.items() } # print('Tuck state:\n {}\nTucking constraints:\n {}'.format('\n '.join(['{}: {}'.format(k, v) for k, v in self._resting_pose.items()]), '\n '.join(tucking_constraints.keys()))) # tucking_constraints.update(self.taxi_constraints) self.use_camera = camera_path is not None if camera_path is not None: self._poi_pos = gm.Symbol('poi') poi = gm.point3(1.5, 0.5, 0.0) + gm.vector3( 0, self._poi_pos * 2.0, 0) camera = km.get_data(camera_path) cam_to_poi = poi - gm.pos_of(camera.pose) lookat_dot = 1 - gm.dot_product(gm.x_of(camera.pose), cam_to_poi) / gm.norm(cam_to_poi) tucking_constraints['sweeping gaze'] = SC(-lookat_dot * 5, -lookat_dot * 5, 1, lookat_dot) symbols = set() for c in tucking_constraints.values(): symbols |= gm.free_symbols(c.expr) joint_symbols = { s for s in symbols if gm.get_symbol_type(s) != gm.TYPE_UNKNOWN } controlled_symbols = {gm.DiffSymbol(s) for s in joint_symbols} hard_constraints = km.get_constraints_by_symbols( symbols.union(controlled_symbols)) controlled_values, hard_constraints = generate_controlled_values( hard_constraints, controlled_symbols) controlled_values = depth_weight_controlled_values( km, controlled_values) self.qp = TQPB(hard_constraints, tucking_constraints, controlled_values) self._start = Time.now()
def test_fixed_joint(self): ks = ArticulationModel() a = Position('a') b = Position('b') parent_pose = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5)) joint_transform = translation3(7, -5, 33) child_pose = parent_pose * joint_transform ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose))) ks.apply_operation('create child', CreateComplexObject(Path('child'), KinematicLink('', se.eye(4)))) self.assertTrue(ks.has_data('parent/pose')) self.assertTrue(ks.has_data('child/pose')) ks.apply_operation('connect parent child', SetFixedJoint(Path('parent/pose'), Path('child/pose'), Path('fixed_joint'), joint_transform)) self.assertTrue(ks.has_data('fixed_joint')) self.assertEquals(ks.get_data('child/pose'), child_pose)
def test_revolute_and_continuous_joint(self): ks = ArticulationModel() a = Position('a') b = Position('b') c = Position('c') parent_pose = frame3_axis_angle(vector3(0,1,0), a, point3(0, b, 5)) joint_transform = translation3(7, -5, 33) axis = vector3(1, -3, 7) axis = axis / norm(axis) position = c child_pose = parent_pose * joint_transform * rotation3_axis_angle(axis, position) ks.apply_operation('create parent', CreateComplexObject(Path('parent'), KinematicLink('', parent_pose))) ks.apply_operation('create child', CreateComplexObject(Path('child'), KinematicLink('', se.eye(4)))) self.assertTrue(ks.has_data('parent/pose')) self.assertTrue(ks.has_data('child/pose')) ks.apply_operation('connect parent child', SetRevoluteJoint(Path('parent/pose'), Path('child/pose'), Path('fixed_joint'), joint_transform, axis, position, -1, 2, 0.5)) self.assertTrue(ks.has_data('fixed_joint')) self.assertEquals(ks.get_data('child/pose'), child_pose) ks.remove_operation('connect parent child') ks.apply_operation('connect parent child', SetContinuousJoint(Path('parent/pose'), Path('child/pose'), Path('fixed_joint'), joint_transform, axis, position, 0.5))
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 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 })
def post_physics_update(self, simulator, deltaT): """Implements post physics step behavior. :type simulator: BasicSimulator :type deltaT: float """ if not self._enabled: return self._last_update += deltaT if self._last_update >= self._update_wait: self._last_update = 0 cf_tuple = self.multibody.get_link_state(self.camera_link).worldFrame camera_frame = gm.frame3_quaternion(cf_tuple.position.x, cf_tuple.position.y, cf_tuple.position.z, *cf_tuple.quaternion) cov_proj = gm.rot_of(camera_frame)[:3, :3] inv_cov_proj = cov_proj.T out = PSAMsg() if self.visualizer is not None: self.visualizer.begin_draw_cycle() poses = [] for name, body in simulator.bodies.items(): if body == self.multibody: continue if isinstance(body, MultiBody): poses_to_process = [('{}/{}'.format(name, l), body.get_link_state(l).worldFrame) for l in body.links] else: poses_to_process = [(name, body.pose())] for pname, pose in poses_to_process: if not pname in self.message_templates: msg = PoseStampedMsg() msg.header.frame_id = pname self.message_templates[pname] = msg else: msg = self.message_templates[pname] obj_pos = gm.point3(*pose.position) c2o = obj_pos - gm.pos_of(camera_frame) dist = gm.norm(c2o) if dist < self.far and dist > self.near and gm.dot_product(c2o, gm.x_of(camera_frame)) > gm.cos(self.fov * 0.5) * dist: noise = 2 ** (self.noise_exp * dist) - 1 (n_quat, ) = np_random_quat_normal(1, 0, noise) (n_trans, ) = np_random_normal_offset(1, 0, noise) n_pose = pb.Transform(pb.Quaternion(*pose.quaternion), pb.Vector3(*pose.position)) *\ pb.Transform(pb.Quaternion(*n_quat), pb.Vector3(*n_trans[:3])) if self.visualizer is not None: poses.append(transform_to_matrix(n_pose)) msg.pose.position.x = n_pose.origin.x msg.pose.position.y = n_pose.origin.y msg.pose.position.z = n_pose.origin.z msg.pose.orientation.x = n_pose.rotation.x msg.pose.orientation.y = n_pose.rotation.y msg.pose.orientation.z = n_pose.rotation.z msg.pose.orientation.w = n_pose.rotation.w out.poses.append(msg) self.publisher.publish(out) if self.visualizer is not None: self.visualizer.draw_poses('debug', gm.se.eye(4), 0.1, 0.02, poses) self.visualizer.render()
def P(n, x, i): return Position('{}_{}_{}'.format(n, x, i)) if __name__ == '__main__': iterations = 200 rec_d = ValueRecorder('Evaluation Speed Comparison', ('direct', 'b'), ('llvm', 'm')) x_labels = [] for x in tqdm(range(1, 101)): y = x * 1 #10 frames = [frame3_rpy(P('rr', x, i), P('rp', x, i), P('ry', x, i), point3(P('x', x, i), P('y', x, i), P('z', x, i))) for i in range(y)] matrix = frames[0] for f in frames: matrix = matrix.col_join(f) x_labels.append(len(matrix.free_symbols)) cython_m = llvm.speed_up(matrix, matrix.free_symbols) avg_d = 0.0 avg_l = 0.0 for i in range(iterations): state = {s: random.random() for s in matrix.free_symbols}
import numpy as np import matplotlib.pyplot as plt from kineverse.visualization.plotting import hsv_to_rgb, \ rgb_to_hex if __name__ == '__main__': a, b = [gm.Position(x) for x in 'ab'] l = 2 a_in_w = gm.dot(gm.translation3(0, 0, 2), gm.translation3(0, 0, -a)) d_in_a = gm.rotation3_axis_angle(gm.vector3(0, 1, 0), gm.acos(a / l)) d_in_w = gm.dot(a_in_w, d_in_a) A = gm.pos_of(a_in_w) B = gm.dot(d_in_w, gm.point3(0, 0, l)) C = gm.dot(d_in_w, gm.point3(0, 0, l * 0.5)) D = gm.dot(d_in_w, gm.point3(0, 0, l * 0.25)) E = gm.dot(d_in_w, gm.point3(0, 0, l * 0.75)) lock_bound = gm.alg_not( gm.alg_and(gm.less_than(b, 0.3), gm.less_than(1.99, a))) # PLOT OF MOVEMENT As = [] Bs = [] Cs = [] Ds = [] Es = []
'r_wrist_roll_joint': 0, # 'torso_lift_joint' : 0.16825 } resting_pose = {gm.Position(Path(f'pr2/{n}')): v for n, v in resting_pose.items()} # Object stuff grasp_poses = {} for p, pose_params in body_paths.items(): if not km.has_data(p): print(f'Link {p} is not part of articulation model') exit(1) if type(pose_params) is not list and type(pose_params) is not tuple: print(f'Grasp pose for {p} is not a list.') exit(1) if len(pose_params) == 6: grasp_poses[Path(p)] = gm.frame3_rpy(*pose_params[-3:], gm.point3(*pose_params[:3])) elif len(pose_params) == 7: grasp_poses[Path(p)] = gm.frame3_quaternion(*pose_params) else: print(f'Grasp pose of {p} has {len(pose_params)} parameters but only 6 or 7 are permissable.') exit(1) monitoring_threshold = rospy.get_param('~m_threshold', 0.6) pr2_commander = PR2VelCommandProcessor(Path('pr2'), '/pr2_vel_controller/command', robot_controlled_symbols, '/base_controller/command', base_symbols, reference_frame=reference_frame)
try: location_x = rospy.get_param('/nobilia_x') location_y = rospy.get_param('/nobilia_y') location_z = rospy.get_param('/nobilia_z') location_yaw = rospy.get_param('/nobilia_yaw') except KeyError as e: print( f'Failed to get nobilia localization params. Original error:\n{e}') exit() reference_frame = rospy.get_param('~reference_frame', 'world') grab_from_tf = rospy.get_param('~grab_from_tf', False) grab_rate = rospy.get_param('~grab_rate', 20.0) shelf_location = gm.point3(location_x, location_y, location_z) # origin_pose = gm.frame3_rpy(0, 0, 0, shelf_location) origin_pose = gm.frame3_rpy(0, 0, location_yaw, shelf_location) create_nobilia_shelf(km, Path('nobilia'), origin_pose, parent_path=Path(reference_frame)) km.clean_structure() km.dispatch_events() shelf = km.get_data('nobilia') tracker = Kineverse6DEKFTracker( km,