Exemple #1
0
    def __init__(self,
                 tracker: Kineverse6DEKFTracker,
                 model=None,
                 model_path=None,
                 urdf_param='/ekf_description_check'):
        self.tracker = tracker
        self.last_observation = {}
        self.last_update = None
        self.last_control = {s: 0.0 for s in self.tracker.controls}
        self.str_controls = {str(s) for s in self.tracker.controls}

        self.vis = ROSVisualizer('~vis', 'world')

        if model is not None:
            self.broadcaster = ModelTFBroadcaster_URDF(urdf_param, model_path,
                                                       model, Path('ekf'))
        else:
            self.broadcaster = None

        self.pub_state = rospy.Publisher('~state_estimate',
                                         ValueMapMsg,
                                         queue_size=1,
                                         tcp_nodelay=True)
        self.sub_observations = rospy.Subscriber('~observations',
                                                 TransformStampedArrayMsg,
                                                 callback=self.cb_obs,
                                                 queue_size=1)
        self.sub_controls = rospy.Subscriber('~controls',
                                             ValueMapMsg,
                                             callback=self.cb_controls,
                                             queue_size=1)
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
Exemple #3
0
    def __init__(self,
                 tracker: Kineverse6DQPTracker,
                 model=None,
                 model_path=None,
                 reference_frame='world',
                 urdf_param='/qp_description_check',
                 update_freq=30,
                 observation_alias=None):
        self.tracker = tracker
        self.last_observation = 0
        self.last_update = 0
        self.reference_frame = reference_frame
        self.observation_aliases = {o: o for o in tracker.observation_names}
        if observation_alias is not None:
            for path, alias in observation_alias.items():
                if path in self.observation_aliases:
                    self.observation_aliases[alias] = path

        self.str_controls = {str(s) for s in self.tracker.get_controls()}

        self.vis = ROSVisualizer('~vis', reference_frame)

        if model is not None:
            self.broadcaster = ModelTFBroadcaster_URDF(urdf_param, model_path,
                                                       model, Path('ekf'))
        else:
            self.broadcaster = None

        self.tf_buffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tf_buffer)

        self.pub_state = rospy.Publisher('~state_estimate',
                                         ValueMapMsg,
                                         queue_size=1,
                                         tcp_nodelay=True)
        self.sub_observations = rospy.Subscriber('~observations',
                                                 TransformStampedArrayMsg,
                                                 callback=self.cb_obs,
                                                 queue_size=1)
        self.sub_controls = rospy.Subscriber('~controls',
                                             ValueMapMsg,
                                             callback=self.cb_controls,
                                             queue_size=1)

        self.timer = rospy.Timer(rospy.Duration(1 / update_freq),
                                 self.cb_update)
if __name__ == '__main__':
    rospy.init_node('iiwa_kinematic_sim')

    rosparam_description = rospy.get_param('/robot_description', None)
    if rosparam_description is None:
        with open(
                res_pkg_path(
                    'package://kineverse_experiment_world/urdf/iiwa_wsg_50.urdf'
                ), 'r') as f:
            rosparam_description = f.read()

    urdf = load_urdf_str(rosparam_description)

    km = GeometryModel()

    load_urdf(km, Path('iiwa'), urdf)

    km.clean_structure()
    km.dispatch_events()

    sim = KineverseKinematicSim(km,
                                Path('iiwa'),
                                state_topic='/iiwa/joint_states')

    sorted_names = sorted(sim.state_info.keys())
    js_msg = JointStateMsg()
    js_msg.name = sorted_names

    def cb_pos_array_command(msg):
        js_msg.header.stamp = rospy.Time.now()
        js_msg.position = msg.data
    vis = ROSBPBVisualizer('~vis', base_frame='world')

    km = GeometryModel()
    robot_type = rospy.get_param('~robot', 'pr2')
    if robot_type.lower() == 'pr2':
        robot_urdf = load_urdf_file('package://iai_pr2_description/robots/pr2_calibrated_with_ft2.xml')
    elif robot_type.lower() == 'hsrb':
        robot_urdf = load_urdf_file('package://hsr_description/robots/hsrb4s.obj.urdf')
    elif robot_type.lower() == 'fetch':
        robot_urdf = load_urdf_file('package://fetch_description/robots/fetch.urdf')
    else:
        print(f'Unknown robot {robot_type}')
        exit(1)

    robot_name = robot_urdf.name
    robot_path = Path(robot_name)
    load_urdf(km, robot_path, robot_urdf, Path('world'))

    km.clean_structure()

    model_name = load_model(km, 
                            rospy.get_param('~model', 'nobilia'),
                            'world',
                            1.0, 0, 0.7, rospy.get_param('~yaw', 0.57))
    km.clean_structure()    
    km.dispatch_events()

    print('\n'.join(km.timeline_tags.keys()))

    if rospy.get_param('~use_base', False):
        if robot_type.lower() == 'fetch':
Exemple #6
0
    observations = rospy.get_param('~features', None)
    num_samples = rospy.get_param('~samples', 5)

    if type(observations) is not dict:
        print(type(observations))
        print(
            'Required parameter ~features needs to be a dict mapping tracked model paths to observation names'
        )
        exit(1)

    km = GeometryModel()

    model_name = load_localized_model(km, model_path, reference_frame)

    km.clean_structure()
    km.dispatch_events()

    model = km.get_data(model_name)
    tracker = Kineverse6DQPTracker(km,
                                   observations.keys(),
                                   num_samples=num_samples)

    node = ROSQPEManager(tracker,
                         model,
                         Path(model_name),
                         reference_frame,
                         observation_alias=observations)

    while not rospy.is_shutdown():
        rospy.sleep(0.1)
def main(create_figure=False,
         vis_mode=False,
         log_csv=True,
         min_n_dof=1,
         samples=300,
         n_observations=25,
         noise_lin=0.2,
         noise_ang=30,
         noise_steps=5):

    wait_duration = rospy.Duration(0.1)

    vis = ROSBPBVisualizer('ekf_vis', 'world') if vis_mode != 'none' else None
    km = GeometryModel()

    with open(res_pkg_path('package://iai_kitchen/urdf_obj/IAI_kitchen.urdf'),
              'r') as urdf_file:
        urdf_kitchen_str = urdf_file.read()
        kitchen_model = urdf_filler(
            URDF.from_xml_string(hacky_urdf_parser_fix(urdf_kitchen_str)))
        load_urdf(km, Path('kitchen'), kitchen_model)

    km.clean_structure()
    km.dispatch_events()

    kitchen = km.get_data('kitchen')

    tracking_pools = []
    for name, link in kitchen.links.items():
        symbols = gm.free_symbols(link.pose)
        if len(symbols) == 0:
            continue

        for x in range(len(tracking_pools)):
            syms, l = tracking_pools[x]
            if len(symbols.intersection(syms)
                   ) != 0:  # BAD ALGORITHM, DOES NOT CORRECTLY IDENTIFY SETS
                tracking_pools[x] = (syms.union(symbols),
                                     l + [(name, link.pose)])
                break
        else:
            tracking_pools.append((symbols, [(name, link.pose)]))

    # tracking_pools = [tracking_pools[7]]
    # print('Identified {} tracking pools:\n{}'.format(len(tracking_pools), tracking_pools))

    all_ekfs = [
        EKFModel(dict(poses), km.get_constraints_by_symbols(symbols))
        for symbols, poses in tracking_pools
    ]  # np.eye(len(symbols)) * 0.001
    print('Created {} EKF models'.format(len(all_ekfs)))
    print('\n'.join(str(e) for e in all_ekfs))

    # Sanity constraint
    min_n_dof = min(min_n_dof, len(all_ekfs))

    iteration_times = []

    for u in range(min_n_dof, len(all_ekfs) + 1):
        if rospy.is_shutdown():
            break

        ekfs = all_ekfs[:u]

        observed_poses = {}
        for ekf in ekfs:
            for link_name, _ in ekf.takers:
                observed_poses[link_name] = kitchen.links[link_name].pose
        names, poses = zip(*sorted(observed_poses.items()))

        state_symbols = union([gm.free_symbols(p) for p in poses])
        ordered_state_vars = [
            s for _, s in sorted((str(s), s) for s in state_symbols)
        ]

        state_constraints = {}
        for n, c in km.get_constraints_by_symbols(state_symbols).items():
            if gm.is_symbol(c.expr):
                s = gm.free_symbols(c.expr).pop()
                fs = gm.free_symbols(c.lower).union(gm.free_symbols(c.upper))
                if len(fs.difference({s})) == 0:
                    state_constraints[s] = (float(gm.subs(c.lower, {s: 0})),
                                            float(gm.subs(c.upper, {s: 0})))

        state_bounds = np.array([
            state_constraints[s]
            if s in state_constraints else [-np.pi * 0.5, np.pi * 0.5]
            for s in ordered_state_vars
        ])

        state_fn = gm.speed_up(gm.vstack(*poses), ordered_state_vars)
        subworld = km.get_active_geometry(state_symbols)

        # Generate observation noise
        print('Generating R matrices...')
        n_cov_obs = 400
        full_log = []

        dof_iters = []

        # EXPERIMENT
        for lin_std, ang_std in [(noise_lin, noise_ang * (np.pi / 180.0))]:
            # zip(np.linspace(0, noise_lin, noise_steps),
            #     np.linspace(0, noise_ang * (np.pi / 180.0), noise_steps)):
            if rospy.is_shutdown():
                break
            # INITIALIZE SENSOR MODEL
            training_obs = []
            state = np.random.uniform(state_bounds.T[0], state_bounds.T[1])
            observations = state_fn.call2(state)

            for _ in range(n_cov_obs):
                noisy_obs = {}
                for x, noise in enumerate([
                        t.dot(r) for t, r in zip(
                            random_normal_translation(len(poses), 0, lin_std),
                            random_rot_normal(len(poses), 0, ang_std))
                ]):
                    noisy_obs[names[x]] = observations[x * 4:x * 4 +
                                                       4, :4].dot(noise)
                training_obs.append(noisy_obs)

            for ekf in ekfs:
                ekf.generate_R(training_obs)
                # ekf.set_R(np.eye(len(ekf.ordered_vars)) * 0.1)

            # Generate figure
            gridsize = (4, samples)
            plot_size = (4, 4)
            fig = plt.figure(figsize=(gridsize[1] * plot_size[0], gridsize[0] *
                                      plot_size[1])) if create_figure else None

            gt_states = []
            states = [[] for x in range(samples)]
            variances = [[] for x in range(samples)]
            e_obs = [[] for x in range(samples)]

            print('Starting iterations')
            for k in tqdm(range(samples)):
                if rospy.is_shutdown():
                    break

                state = np.random.uniform(state_bounds.T[0], state_bounds.T[1])
                gt_states.append(state)
                observations = state_fn.call2(state).copy()
                gt_obs_d = {
                    n: observations[x * 4:x * 4 + 4, :4]
                    for x, n in enumerate(names)
                }
                subworld.update_world(dict(zip(ordered_state_vars, state)))

                if vis_mode == 'iter' or vis_mode == 'io':
                    vis.begin_draw_cycle('gt', 'noise', 'estimate', 't_n',
                                         't0')
                    vis.draw_world('gt', subworld, g=0, b=0)
                    vis.render('gt')

                estimates = []
                for ekf in ekfs:
                    particle = ekf.spawn_particle()
                    estimates.append(particle)

                initial_state = dict(
                    sum([[(s, v) for s, v in zip(ekf.ordered_vars, e.state)]
                         for e, ekf in zip(estimates, ekfs)], []))
                initial_state = np.array(
                    [initial_state[s] for s in ordered_state_vars])
                if initial_state.min() < state_bounds.T[0].min(
                ) or initial_state.max() > state_bounds.T[1].max():
                    raise Exception(
                        'Estimate initialization is out of bounds: {}'.format(
                            np.vstack([initial_state, state_bounds.T]).T))
                initial_delta = state - initial_state

                for y in range(n_observations):
                    # Add noise to observations
                    noisy_obs = {}
                    for x, noise in enumerate([
                            t.dot(r) for t, r in zip(
                                random_normal_translation(
                                    len(poses), 0, lin_std),
                                random_rot_normal(len(poses), 0, ang_std))
                    ]):
                        noisy_obs[names[x]] = observations[x * 4:x * 4 +
                                                           4, :4].dot(noise)

                    if vis_mode in {'iter', 'iter-trail'} or (vis_mode == 'io'
                                                              and y == 0):
                        for n, t in noisy_obs.items():
                            subworld.named_objects[Path(
                                ('kitchen', 'links', n))].np_transform = t
                        if vis_mode != 'iter-trail':
                            vis.begin_draw_cycle('noise')
                        vis.draw_world('noise', subworld, r=0, g=0, a=0.1)
                        vis.render('noise')

                    start_time = time()
                    for estimate, ekf in zip(estimates, ekfs):
                        if y > 0:
                            control = np.zeros(len(ekf.ordered_controls))
                            estimate.state, estimate.cov = ekf.predict(
                                estimate.state.flatten(), estimate.cov,
                                control)
                            obs_vector = ekf.gen_obs_vector(noisy_obs)
                            estimate.state, estimate.cov = ekf.update(
                                estimate.state, estimate.cov,
                                ekf.gen_obs_vector(noisy_obs))

                            if vis_mode in {'iter', 'iter-trail'}:
                                subworld.update_world({
                                    s: v
                                    for s, v in zip(ekf.ordered_vars,
                                                    estimate.state)
                                })
                        else:
                            obs_vector = ekf.gen_obs_vector(noisy_obs)

                            for _ in range(1):
                                h_prime = ekf.h_prime_fn.call2(estimate.state)
                                obs_delta = obs_vector.reshape(
                                    (len(obs_vector), 1)) - ekf.h_fn.call2(
                                        estimate.state)
                                estimate.state += (h_prime.T.dot(obs_delta) *
                                                   0.1).reshape(
                                                       estimate.state.shape)

                            if vis_mode in {'iter', 'io'}:
                                subworld.update_world({
                                    s: v
                                    for s, v in zip(ekf.ordered_vars,
                                                    estimate.state)
                                })

                    if vis_mode != 'none' and y == 0:
                        vis.draw_world('t0', subworld, b=0, a=1)
                        vis.render('t0')
                    elif vis_mode in {'iter', 'iter-trail'}:
                        if vis_mode != 'iter-trail':
                            vis.begin_draw_cycle('t_n')
                        vis.draw_world('t_n', subworld, b=0, a=1)
                        vis.render('t_n')

                    if log_csv or fig is not None:
                        e_state_d = dict(
                            sum([[(s, v)
                                  for s, v in zip(ekf.ordered_vars, e.state)]
                                 for e, ekf in zip(estimates, ekfs)], []))
                        covs = dict(
                            sum([[(s, v) for s, v in zip(
                                ekf.ordered_vars, np.sqrt(np.trace(e.cov)))]
                                 for e, ekf in zip(estimates, ekfs)], []))
                        e_state = np.hstack([
                            e_state_d[s] for s in ordered_state_vars
                        ]).reshape((len(e_state_d), ))

                        if log_csv:
                            full_log.append(
                                np.hstack(
                                    ([lin_std,
                                      ang_std], state, e_state.flatten(),
                                     np.array([
                                         covs[s] for s in ordered_state_vars
                                     ]))))

                        if fig is not None:
                            e_obs[k].append(
                                np.array([
                                    np.abs(
                                        ekf.gen_obs_vector(gt_obs_d) -
                                        ekf.h_fn.call2(e.state)).max()
                                    for e, ekf in zip(estimates, ekfs)
                                ]))
                            states[k].append(e_state)
                            variances[k].append(
                                np.array([covs[s]
                                          for s in ordered_state_vars]))
                else:
                    if vis_mode == 'io':
                        for estimate, ekf in zip(estimates, ekfs):
                            subworld.update_world({
                                s: v
                                for s, v in zip(ekf.ordered_vars,
                                                estimate.state)
                            })

                        vis.draw_world('t_n', subworld, r=0, b=0, a=1)
                        vis.render('t_n')

                    dof_iters.append(time() - start_time)

            if fig is not None:
                axes = [
                    plt.subplot2grid(gridsize, (y, 0), colspan=1, rowspan=1)
                    for y in range(gridsize[0])
                ]
                axes = np.array(
                    sum([[
                        plt.subplot2grid(gridsize, (y, x),
                                         colspan=1,
                                         rowspan=1,
                                         sharey=axes[y])
                        for y in range(gridsize[0])
                    ] for x in range(1, gridsize[1])], axes)).reshape(
                        (gridsize[1], gridsize[0]))

                for x, (gt_s, state, variance, obs_delta,
                        (ax_s, ax_d, ax_o, ax_v)) in enumerate(
                            zip(gt_states, states, variances, e_obs, axes)):

                    for y in gt_s:
                        ax_s.axhline(y, xmin=0.97, xmax=1.02)

                    ax_s.set_title('State; Sample: {}'.format(x))
                    ax_d.set_title('Delta from GT; Sample: {}'.format(x))
                    ax_o.set_title('Max Delta in Obs; Sample: {}'.format(x))
                    ax_v.set_title('Standard Deviation; Sample: {}'.format(x))
                    ax_s.plot(state)
                    ax_d.plot(gt_s - np.vstack(state))
                    ax_o.plot(obs_delta)
                    ax_v.plot(variance)
                    ax_s.grid(True)
                    ax_d.grid(True)
                    ax_o.grid(True)
                    ax_v.grid(True)

                fig.tight_layout()
                plt.savefig(
                    res_pkg_path(
                        'package://kineverse_experiment_world/test/ekf_object_tracker_{}_{}.png'
                        .format(lin_std, ang_std)))

        iteration_times.append(dof_iters)

        if log_csv:
            df = pd.DataFrame(
                columns=['lin_std', 'ang_std'] +
                ['gt_{}'.format(x) for x in range(len(state_symbols))] +
                ['ft_{}'.format(x) for x in range(len(state_symbols))] +
                ['var_{}'.format(x) for x in range(len(state_symbols))],
                data=full_log)
            df.to_csv(res_pkg_path(
                'package://kineverse_experiment_world/test/ekf_object_tracker.csv'
            ),
                      index=False)

    df = pd.DataFrame(
        columns=[str(x) for x in range(1,
                                       len(iteration_times) + 1)],
        data=np.vstack(iteration_times).T)
    df.to_csv(res_pkg_path(
        'package://kineverse_experiment_world/test/ekf_object_tracker_performance.csv'
    ),
              index=False)
    visualizer.render('world')


def handle_sigint(*args):
    dpg.stop_dearpygui()


if __name__ == '__main__':
    rospy.init_node('nobilia_shelf_test')

    km = GeometryModel()
    vis = ROSBPBVisualizer('kineverse/nobilia/vis', base_frame='world')

    origin_pose = gm.translation3(x_loc_sym, 0, 0)

    debug = create_nobilia_shelf(km, Path('nobilia'), origin_pose)

    km.clean_structure()
    km.dispatch_events()

    shelf = km.get_data('nobilia')

    shelf_pos = shelf.joints['hinge'].position

    world = km.get_active_geometry({shelf_pos
                                    }.union(gm.free_symbols(origin_pose)))

    params = debug.tuning_params

    with dpg.window(label='Parameter settings'):
        for s, p in debug.tuning_params.items():
#!/usr/bin/env python

import kineverse.gradients.common_math as cm

from kineverse.gradients.gradient_math import point3, vector3, translation3, frame3_axis_angle, Position
from kineverse.model.geometry_model import GeometryModel, RigidBody, Geometry, Path
from kineverse.operations.basic_operations import CreateComplexObject as OP_CCO, \
                                                  CreateSingleValue   as OP_CSV
from kineverse.operations.special_kinematics import SetBallJoint as OP_SBJ

if __name__ == '__main__':
    km = GeometryModel()

    geom_head = Geometry(
        Path('head'),
        cm.eye(4),
        'mesh',
        mesh='package://kineverse_experiment_world/urdf/faucet_head.obj')
    rb_head = RigidBody(Path('world'),
                        cm.eye(4),
                        geometry={0: geom_head},
                        collision={0: geom_head})

    geom_base = Geometry(
        Path('base'),
        cm.eye(4),
        'mesh',
        mesh='package://kineverse_experiment_world/urdf/faucet_base.obj')
    rb_base = RigidBody(Path('world'),
                        cm.eye(4),
                        geometry={0: geom_base},
Exemple #10
0
from kineverse.motion.integrator import CommandIntegrator
from kineverse.visualization.bpb_visualizer import ROSBPBVisualizer
from kineverse.visualization.plotting import draw_recorders, split_recorders
from kineverse.visualization.trajectory_visualizer import TrajectoryVisualizer

from urdf_parser_py.urdf import URDF

if __name__ == '__main__':
    rospy.init_node('microwave_sandbox')

    micro_urdf = URDF.from_xml_file(
        res_pkg_path('package://kineverse/urdf/microwave.urdf'))

    km = EventModel()
    #km = GeometryModel()
    load_urdf(km, Path('microwave'), micro_urdf)

    traj_vis = TrajectoryVisualizer(ROSBPBVisualizer('/bullet_test', '/map'))
    traj_vis.add_articulated_object(micro_urdf, km.get_data('microwave'))

    door_position = km.get_data('microwave/joints/door_joint').position
    button_position = km.get_data('microwave/joints/button_joint').position
    button_min = -0.02
    door_velocity = get_diff_symbol(door_position)
    door_accel = get_diff_symbol(door_velocity)
    point_of_release = 0.8 * button_min
    button_pushed = less_than(button_position, point_of_release)
    condition = alg_and(less_than(door_position, 0.1), button_pushed)

    spring_constraint = Constraint((2 * condition)**(-100 * door_position),
                                   1e9, door_accel)
Exemple #11
0
    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,
        # [Path(f'nobilia/markers/{name}') for name in shelf.markers.keys()], #
        # [Path(f'nobilia/markers/body')],
        {Path(f'nobilia/markers/top_panel'): 'obs_shelf_body'},
        #{x: x for x in gm.free_symbols(origin_pose)},
        noise_estimation_observations=20,
        estimate_init_steps=1000)
Exemple #12
0
    ) == '.xml':
        urdf_model = load_urdf_file(description)
    else:
        if not rospy.has_param(description):
            print(
                f'Description is supposed to be located at "{description}" but that parameter does not exist'
            )
        urdf_model = load_urdf_str(rospy.get_param(description))

    name = rospy.get_param('~name', urdf_model.name)
    reference_frame = rospy.get_param('~reference_frame', 'world')

    km = GeometryModel()

    load_urdf(km,
              Path(name),
              urdf_model,
              reference_frame,
              joint_prefix='',
              root_transform=root_transform)

    km.clean_structure()

    if special_base is not None:
        if special_base == 'omni':
            insert_omni_base(km, Path(name), urdf_model.get_root(),
                             reference_frame)
        elif special_base == 'diff':
            insert_diff_base(km, Path(name), urdf_model.get_root(),
                             reference_frame)
Exemple #13
0
            )
            exit(1)
        if len(root_transform) == 6:
            root_transform = gm.frame3_rpy(root_transform[3],
                                           root_transform[4],
                                           root_transform[5],
                                           root_transform[:3])
        elif len(root_transform) == 7:
            root_transform = gm.frame3_rpy(*root_transform)
        else:
            print(
                'root_transform needs to encode transform eiter as xyzrpy, or as xyzqxqyqzqw'
            )
            exit(1)

    km = GeometryModel()

    create_nobilia_shelf(km, Path('nobilia'), root_transform)

    km.clean_structure()
    km.dispatch_events()

    sim = KineverseKinematicSim(km,
                                Path('nobilia'),
                                '/nobilia_description',
                                command_type=ValueMapMsg)

    print(f'Running kinematics sim for nobilia shelf')

    while not rospy.is_shutdown():
        rospy.sleep(1)