def load_localized_model(km, model_path, reference_frame): if model_path != 'nobilia': urdf_model = load_urdf_file(model_path) return load_model(km, model_path, reference_frame, *[gm.Position(f'{urdf_model.name}_location_{x}') for x in 'xyz'], gm.Position(f'{urdf_model.name}_location_yaw')) else: return load_model(km, model_path, reference_frame, *[gm.Position(f'nobilia_location_{x}') for x in 'xyz'], gm.Position('nobilia_location_yaw'))
def cb_robot_js(self, js_msg): if self._state_cb is None: return state_update = {} for x, name in enumerate(js_msg.name): if len(js_msg.position) > x: state_update[gm.Position(self.robot_prefix + (name, ))] = js_msg.position[x] if len(js_msg.velocity) > x: state_update[gm.Velocity(self.robot_prefix + (name, ))] = js_msg.velocity[x] if self.base_aliases is not None: try: trans = self.tf_buffer.lookup_transform( self.reference_frame, 'base_footprint', rospy.Time(0)) state_update[self.base_aliases.ap] = math.acos( trans.transform.rotation.w) * 2 state_update[ self.base_aliases.xp] = trans.transform.translation.x state_update[ self.base_aliases.yp] = trans.transform.translation.y state_update[self.base_aliases.av] = 0 state_update[self.base_aliases.xv] = 0 state_update[self.base_aliases.yv] = 0 except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: print( f'Exception raised while looking up {self.reference_frame} -> base_footprint:\n{e}' ) return self._state_cb(state_update)
def pr2_setup(km, pr2_path): start_pose = {'l_elbow_flex_joint' : -2.1213, 'l_shoulder_lift_joint': 1.2963, 'l_wrist_flex_joint' : -1.16, 'l_shoulder_pan_joint': 0.7, 'r_shoulder_pan_joint': -1.0, 'r_shoulder_lift_joint': 0.9, 'r_upper_arm_roll_joint': -1.2, 'r_elbow_flex_joint' : -2.1213, 'r_wrist_flex_joint' : -1.05, 'r_forearm_roll_joint': 3.14, 'r_wrist_roll_joint': 0, 'torso_lift_joint' : 0.16825} start_pose = {gm.Position(Path(f'{pr2_path}/{n}')): v for n, v in start_pose.items()} joint_symbols = [j.position for j in km.get_data(f'{pr2_path}/joints').values() if hasattr(j, 'position') and gm.is_symbol(j.position)] robot_controlled_symbols = {gm.DiffSymbol(j) for j in joint_symbols} # if 'torso' not in str(j)} blacklist = {gm.Velocity(Path(f'{pr2_path}/torso_lift_joint'))} return joint_symbols, \ robot_controlled_symbols, \ start_pose, \ km.get_data(pr2_path).links['r_gripper_tool_frame'], \ blacklist
resting_pose = { # 'l_elbow_flex_joint' : -2.1213, # 'l_shoulder_lift_joint': 1.2963, # 'l_wrist_flex_joint' : -1.16, 'r_shoulder_pan_joint': -1.0, 'r_shoulder_lift_joint': 0.9, 'r_upper_arm_roll_joint': -1.2, 'r_elbow_flex_joint': -2.1213, 'r_wrist_flex_joint': -1.05, 'r_forearm_roll_joint': 3.14, '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() } nav_method = rospy.get_param('~nav_method', 'proj') gripper = PR2GripperWrapper('/r_gripper_controller') pr2_commander = PR2VelCommandProcessor(Path('pr2'), '/pr2_vel_controller/command', robot_controlled_symbols, '/base_controller/command', base_symbols, reference_frame=reference_frame) behavior = ROSPushingBehavior(
robot_controlled_symbols |= {base_joint.l_wheel_vel, base_joint.r_wheel_vel} robot_controlled_symbols.difference_update(gm.DiffSymbol(s) for s in [base_joint.x_pos, base_joint.y_pos, base_joint.a_pos]) blacklist.update(gm.DiffSymbol(s) for s in [base_joint.x_pos, base_joint.y_pos, base_joint.a_pos]) integration_rules = { base_joint.x_pos: base_joint.x_pos + sym_dt * (base_joint.r_wheel_vel * gm.cos(base_joint.a_pos) * base_joint.wheel_radius * 0.5 + base_joint.l_wheel_vel * gm.cos(base_joint.a_pos) * base_joint.wheel_radius * 0.5), base_joint.y_pos: base_joint.y_pos + sym_dt * (base_joint.r_wheel_vel * gm.sin(base_joint.a_pos) * base_joint.wheel_radius * 0.5 + base_joint.l_wheel_vel * gm.sin(base_joint.a_pos) * base_joint.wheel_radius * 0.5), base_joint.a_pos: base_joint.a_pos + sym_dt * (base_joint.r_wheel_vel * (base_joint.wheel_radius / base_joint.wheel_distance) + base_joint.l_wheel_vel * (- base_joint.wheel_radius / base_joint.wheel_distance))} start_pose = {'wrist_roll_joint' : 0.0, 'shoulder_pan_joint' : 0.3, 'elbow_flex_joint' : 1.72, 'forearm_roll_joint' : -1.2, 'upperarm_roll_joint': -1.57, 'wrist_flex_joint' : 1.66, 'shoulder_lift_joint': 1.4, 'torso_lift_joint' : 0.2} start_pose = {gm.Position(Path(f'{robot_path}/{n}')): v for n, v in start_pose.items()} else: joint_symbols, \ robot_controlled_symbols, \ start_pose, \ eef, \ blacklist = generic_setup(km, robot_path, rospy.get_param('~eef', 'gripper_link')) collision_world = km.get_active_geometry(gm.free_symbols(handle.pose).union(gm.free_symbols(eef.pose))) # Init step grasp_in_handle = gm.dot(gm.translation3(0.04, 0, 0), gm.rotation3_rpy(math.pi * 0.5, 0, math.pi)) goal_pose = gm.dot(handle.pose, grasp_in_handle) goal_0_pose = gm.subs(goal_pose, {s: 0 for s in gm.free_symbols(goal_pose)}) start_state = {s: 0 for s in gm.free_symbols(collision_world)}
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 })
else: pushing_controller = PushingController( km, eef_path, part_path, robot_controlled_symbols, start_state, cam_path, use_geom_circulation, None, weights, collision_avoidance_paths=active_parts_to_avoid) if robot in start_poses: start_state.update({ gm.Position(Path(robot) + (k, )): v for k, v in start_poses[robot].items() }) start_state.update({ c.symbol: 0.0 for c in pushing_controller.controlled_values.values() }) # start_state.update({s: 1.84 for s in gm.free_symbols(obj.pose)}) start_state.update({s: 0.4 for s in gm.free_symbols(obj.pose)}) printed_exprs['relative_vel'] = gm.norm( pushing_controller.p_internals.relative_pos) integrator = CommandIntegrator(pushing_controller.qpb, integration_rules,
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))
visualizer = ROSBPBVisualizer('~visualization', base_frame='world') km = GeometryModel() with open( res_pkg_path( 'package://kineverse_experiment_world/urdf/iiwa_wsg_50.urdf'), 'r') as f: iiwa_urdf = urdf_filler( URDF.from_xml_string(hacky_urdf_parser_fix(f.read()))) load_urdf(km, Path('iiwa'), iiwa_urdf) km.clean_structure() km.dispatch_events() door_x, door_y = [gm.Position(f'localization_{x}') for x in 'xy'] create_door(km, Path('door'), 0.5, 0.35, to_world_tf=gm.translation3(door_x, door_y, 0.1)) km.clean_structure() km.dispatch_events() door = km.get_data('door') handle = door.links['handle'] iiwa = km.get_data('iiwa') symbols = gm.free_symbols(door.links['handle'].pose).union( gm.free_symbols(iiwa.links['link_7'].pose))
import kineverse.gradients.gradient_math as gm 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 = []
eef_path = Path(f'pr2/links/{eef_link}') cam_path = Path('pr2/links/head_mount_link') resting_pose = {#'l_elbow_flex_joint' : -2.1213, #'l_shoulder_lift_joint': 1.2963, #'l_wrist_flex_joint' : -1.16, 'r_shoulder_pan_joint': -1.0, 'r_shoulder_lift_joint': 0.9, 'r_upper_arm_roll_joint': -1.2, 'r_elbow_flex_joint' : -2.1213, 'r_wrist_flex_joint' : -1.05, 'r_forearm_roll_joint': 0, '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: