예제 #1
0
def exercise_system_model_with_zero_dof_body():
    bodies = [zero_dof_body(), revolute_body(parent=0)]
    body_lib.set_cb_tree(bodies=bodies)
    model = featherstone.system_model(bodies=bodies)
    assert approx_equal(model.e_kin(), 0.0202765671829)
    assert approx_equal(model.qd_e_kin_scales(), [1.334309])
    #
    qdd = matrix.col_list([(), (0.14, )])
    f_ext = matrix.col_list([(-0.10, 0.30, -0.01, -0.01, 0.01, 0.06),
                             (-0.11, 0.03, -0.07, -0.11, 0.06, 0.08)])
    grav_accn = matrix.col((0.02, -0.13, 0.15, 0.26, -0.16, 0.14))
    #
    tau = model.inverse_dynamics(qdd_array=qdd)
    assert approx_equal(tau, [(), (0.15726977316344815, )])
    qdd2 = model.forward_dynamics_ab(tau_array=tau)
    assert approx_equal(qdd2, qdd)
    #
    tau = model.inverse_dynamics(qdd_array=qdd, f_ext_array=f_ext)
    assert approx_equal(tau, [(), (0.22726977316344815, )])
    qdd2 = model.forward_dynamics_ab(tau_array=tau, f_ext_array=f_ext)
    assert approx_equal(qdd2, qdd)
    #
    tau = model.inverse_dynamics(qdd_array=qdd,
                                 f_ext_array=f_ext,
                                 grav_accn=grav_accn)
    assert approx_equal(tau, [(), (0.59601742875022201, )])
    qdd2 = model.forward_dynamics_ab(tau_array=tau,
                                     f_ext_array=f_ext,
                                     grav_accn=grav_accn)
    assert approx_equal(qdd2, qdd)
    #
    new_q = [
        body.joint.time_step_position(qd=body.qd, delta_t=0.01).get_q()
        for body in model.bodies
    ]
    assert approx_equal(new_q, [(), (0.2581, )])
    new_qd = [
        body.joint.time_step_velocity(qd=body.qd, qdd=qdd_i, delta_t=0.01)
        for body, qdd_i in zip(model.bodies, qdd)
    ]
    assert approx_equal(new_qd, [(), (-0.1886, )])
    for body, q in zip(model.bodies, [(), (13, )]):
        assert approx_equal(body.joint.new_q(q=q).get_q(), q)
    #
    qdd = []
    for body in model.bodies:
        body.qd = body.joint.qd_zero
        qdd.append(body.joint.qdd_zero)
    model.flag_velocities_as_changed()
    tau = model.inverse_dynamics(qdd_array=qdd, f_ext_array=f_ext)
    assert approx_equal(tau, [(), (0.07, )])
    tau0 = model.f_ext_as_tau(f_ext_array=f_ext)
    assert approx_equal(tau0, tau)
예제 #2
0
def exercise_system_model_with_zero_dof_body():
  bodies = [
    zero_dof_body(),
    revolute_body(parent=0)]
  body_lib.set_cb_tree(bodies=bodies)
  model = featherstone.system_model(bodies=bodies)
  assert approx_equal(model.e_kin(), 0.0202765671829)
  assert approx_equal(model.qd_e_kin_scales(), [1.334309])
  #
  qdd = matrix.col_list([
    (),
    (0.14,)])
  f_ext = matrix.col_list([
    (-0.10, 0.30, -0.01, -0.01, 0.01, 0.06),
    (-0.11, 0.03, -0.07, -0.11, 0.06, 0.08)])
  grav_accn = matrix.col((0.02, -0.13, 0.15, 0.26, -0.16, 0.14))
  #
  tau = model.inverse_dynamics(qdd_array=qdd)
  assert approx_equal(tau, [
    (),
    (0.15726977316344815,)])
  qdd2 = model.forward_dynamics_ab(tau_array=tau)
  assert approx_equal(qdd2, qdd)
  #
  tau = model.inverse_dynamics(qdd_array=qdd, f_ext_array=f_ext)
  assert approx_equal(tau, [
    (),
    (0.22726977316344815,)])
  qdd2 = model.forward_dynamics_ab(tau_array=tau, f_ext_array=f_ext)
  assert approx_equal(qdd2, qdd)
  #
  tau = model.inverse_dynamics(
    qdd_array=qdd, f_ext_array=f_ext, grav_accn=grav_accn)
  assert approx_equal(tau, [
    (),
    (0.59601742875022201,)])
  qdd2 = model.forward_dynamics_ab(
    tau_array=tau, f_ext_array=f_ext, grav_accn=grav_accn)
  assert approx_equal(qdd2, qdd)
  #
  new_q = [
    body.joint.time_step_position(qd=body.qd, delta_t=0.01).get_q()
      for body in model.bodies]
  assert approx_equal(new_q, [
    (), (0.2581,)])
  new_qd = [
    body.joint.time_step_velocity(qd=body.qd, qdd=qdd_i, delta_t=0.01)
      for body,qdd_i in zip(model.bodies, qdd)]
  assert approx_equal(new_qd, [
    (),
    (-0.1886,)])
  for body,q in zip(model.bodies, [(),(13,)]):
    assert approx_equal(body.joint.new_q(q=q).get_q(), q)
  #
  qdd = []
  for body in model.bodies:
    body.qd = body.joint.qd_zero
    qdd.append(body.joint.qdd_zero)
  model.flag_velocities_as_changed()
  tau = model.inverse_dynamics(qdd_array=qdd, f_ext_array=f_ext)
  assert approx_equal(tau, [
    (),
    (0.07,)])
  tau0 = model.f_ext_as_tau(f_ext_array=f_ext)
  assert approx_equal(tau0, tau)
예제 #3
0
def exercise_system_model():
    bodies = [
        six_dof_body(),
        spherical_body(),
        revolute_body(parent=1),
        translational_body()
    ]
    body_lib.set_cb_tree(bodies=bodies)
    model = featherstone.system_model(bodies=bodies)
    assert approx_equal(model.e_kin(), 5.10688665235)
    assert approx_equal(model.qd_e_kin_scales(), [
        0.1036643, 0.1054236, 0.1187526, 0.5773503, 0.5773503, 0.5773503,
        0.1749883, 0.2830828, 0.2225619, 1.334309, 1.414214, 1.414214, 1.414214
    ])
    #
    qdd = matrix.col_list([(-0.04, 0.05, 0.23, -0.01, -0.08, 0.04),
                           (0.08, -0.08, -0.01), (0.14, ),
                           (-0.01, -0.34, 0.28)])
    f_ext = matrix.col_list([(-0.10, 0.30, -0.01, -0.01, 0.01, 0.06),
                             (0.28, 0.09, 0.14, 0.23, 0.00, 0.07),
                             (-0.11, 0.03, -0.07, -0.11, 0.06, 0.08),
                             (-0.16, 0.14, -0.33, 0.35, -0.02, -0.20)])
    grav_accn = matrix.col((0.02, -0.13, 0.15, 0.26, -0.16, 0.14))
    #
    qdd0 = [(0, 0, 0, 0, 0, 0), (0, 0, 0), (0, ), (0, 0, 0)]
    for qdd_array in [None, qdd0]:
        tau = model.inverse_dynamics(qdd_array=qdd_array)
        assert approx_equal(
            tau,
            [(3.0067673409496019, -1.9747070164103167, -0.96510418705493095,
              0.62119145987365987, 0.79528549692226591, 0.50582706679253908),
             (-3.5089966946778439, 0.85280077414986188, -1.1466929846982585),
             (-0.68035016279655447, ), (0.0, 0.0, 0.0)])
    qdd2 = model.forward_dynamics_ab(tau_array=tau)
    assert approx_equal(qdd2, qdd0)
    #
    tau = model.inverse_dynamics(qdd_array=qdd)
    assert approx_equal(tau, [(-28.4935967396, -13.9449610757, 37.119813341,
                               3.09036984758, -3.29209848977, 1.51871803584),
                              (22.0723956539, 1.85204188959, -1.96550741514),
                              (0.526685445884, ), (-0.01, -0.34, 0.28)])
    qdd2 = model.forward_dynamics_ab(tau_array=tau)
    assert approx_equal(qdd2, qdd)
    #
    tau = model.inverse_dynamics(qdd_array=qdd, f_ext_array=f_ext)
    assert approx_equal(tau, [(-28.2069898504, -14.1650076325, 38.2656278316,
                               3.24402886492, -3.30833610224, 1.36344785723),
                              (22.6135703212, 2.25806832722, -2.77922603881),
                              (0.596685445884, ), (-0.36, -0.32, 0.48)])
    qdd2 = model.forward_dynamics_ab(tau_array=tau, f_ext_array=f_ext)
    assert approx_equal(qdd2, qdd)
    #
    tau = model.inverse_dynamics(qdd_array=qdd,
                                 f_ext_array=f_ext,
                                 grav_accn=grav_accn)
    assert approx_equal(tau, [(29.0716177639, 1.548329665, -9.90799285557,
                               -2.51132634591, 5.78686348626, -3.77518591503),
                              (-4.70390288163, -1.2432778965, 1.1909533225),
                              (-0.354638347024, ),
                              (0.54782, -0.17957, 0.16733)])
    qdd2 = model.forward_dynamics_ab(tau_array=tau,
                                     f_ext_array=f_ext,
                                     grav_accn=grav_accn)
    assert approx_equal(qdd2, qdd)
    #
    new_q = [
        body.joint.time_step_position(qd=body.qd, delta_t=0.01).get_q()
        for body in model.bodies
    ]
    assert approx_equal(new_q,
                        [(0.18036749, 0.36210928, 0.54329229, -0.7356480,
                          -0.10189282, 0.29788946, 0.2020574),
                         (-0.50329486, -0.33273860, 0.67548709, -0.4239072),
                         (0.2581, ), (-0.1005, 0.2981, 0.2029)])
    new_qd = [
        body.joint.time_step_velocity(qd=body.qd, qdd=qdd_i, delta_t=0.01)
        for body, qdd_i in zip(model.bodies, qdd)
    ]
    assert approx_equal(new_qd,
                        [(0.1796, -0.0195, -0.1577, -0.0501, -0.1908, 0.2904),
                         (0.1208, -0.0808, 0.1099), (-0.1886, ),
                         (-0.0501, -0.1934, 0.2928)])
    for body, q in zip(model.bodies, [(1, 2, 3, 4, 5, 6, 7), (8, 9, 10, 11),
                                      (12, )]):
        assert approx_equal(body.joint.new_q(q=q).get_q(), q)
    #
    qdd = []
    for body in model.bodies:
        body.qd = body.joint.qd_zero
        qdd.append(body.joint.qdd_zero)
    model.flag_velocities_as_changed()
    tau = model.inverse_dynamics(qdd_array=qdd, f_ext_array=f_ext)
    assert approx_equal(tau,
                        [(0.286606889188, -0.220046556736, 1.14581449056,
                          0.153659017347, -0.0162376124727, -0.155270178613),
                         (0.541174667239, 0.406026437633, -0.813718623664),
                         (0.07, ), (-0.35, 0.02, 0.2)])
    tau0 = model.f_ext_as_tau(f_ext_array=f_ext)
    assert approx_equal(tau0, tau)
예제 #4
0
def exercise_system_model():
  bodies = [
    six_dof_body(),
    spherical_body(),
    revolute_body(parent=1),
    translational_body()]
  body_lib.set_cb_tree(bodies=bodies)
  model = featherstone.system_model(bodies=bodies)
  assert approx_equal(model.e_kin(), 5.10688665235)
  assert approx_equal(model.qd_e_kin_scales(), [
    0.1036643, 0.1054236, 0.1187526, 0.5773503, 0.5773503, 0.5773503,
    0.1749883, 0.2830828, 0.2225619,
    1.334309,
    1.414214, 1.414214, 1.414214])
  #
  qdd = matrix.col_list([
    (-0.04,0.05,0.23,-0.01,-0.08,0.04),
    (0.08,-0.08,-0.01),
    (0.14,),
    (-0.01, -0.34, 0.28)])
  f_ext = matrix.col_list([
    (-0.10, 0.30, -0.01, -0.01, 0.01, 0.06),
    (0.28, 0.09, 0.14, 0.23, 0.00, 0.07),
    (-0.11, 0.03, -0.07, -0.11, 0.06, 0.08),
    (-0.16, 0.14, -0.33, 0.35, -0.02, -0.20)])
  grav_accn = matrix.col((0.02, -0.13, 0.15, 0.26, -0.16, 0.14))
  #
  qdd0 = [
    (0,0,0,0,0,0),
    (0,0,0),
    (0,),
    (0,0,0)]
  for qdd_array in [None, qdd0]:
    tau = model.inverse_dynamics(qdd_array=qdd_array)
    assert approx_equal(tau, [
      (3.0067673409496019, -1.9747070164103167, -0.96510418705493095,
       0.62119145987365987, 0.79528549692226591, 0.50582706679253908),
      (-3.5089966946778439, 0.85280077414986188, -1.1466929846982585),
      (-0.68035016279655447,),
      (0.0, 0.0, 0.0)])
  qdd2 = model.forward_dynamics_ab(tau_array=tau)
  assert approx_equal(qdd2, qdd0)
  #
  tau = model.inverse_dynamics(qdd_array=qdd)
  assert approx_equal(tau, [
    (-28.4935967396, -13.9449610757, 37.119813341,
     3.09036984758, -3.29209848977, 1.51871803584),
    (22.0723956539, 1.85204188959, -1.96550741514),
    (0.526685445884,),
    (-0.01,-0.34,0.28)])
  qdd2 = model.forward_dynamics_ab(tau_array=tau)
  assert approx_equal(qdd2, qdd)
  #
  tau = model.inverse_dynamics(qdd_array=qdd, f_ext_array=f_ext)
  assert approx_equal(tau, [
    (-28.2069898504, -14.1650076325, 38.2656278316,
     3.24402886492, -3.30833610224, 1.36344785723),
    (22.6135703212, 2.25806832722, -2.77922603881),
    (0.596685445884,),
    (-0.36,-0.32,0.48)])
  qdd2 = model.forward_dynamics_ab(tau_array=tau, f_ext_array=f_ext)
  assert approx_equal(qdd2, qdd)
  #
  tau = model.inverse_dynamics(
    qdd_array=qdd, f_ext_array=f_ext, grav_accn=grav_accn)
  assert approx_equal(tau, [
    (29.0716177639, 1.548329665, -9.90799285557,
     -2.51132634591, 5.78686348626, -3.77518591503),
    (-4.70390288163, -1.2432778965, 1.1909533225),
    (-0.354638347024,),
    (0.54782, -0.17957, 0.16733)])
  qdd2 = model.forward_dynamics_ab(
    tau_array=tau, f_ext_array=f_ext, grav_accn=grav_accn)
  assert approx_equal(qdd2, qdd)
  #
  new_q = [
    body.joint.time_step_position(qd=body.qd, delta_t=0.01).get_q()
      for body in model.bodies]
  assert approx_equal(new_q, [
    (0.18036749, 0.36210928, 0.54329229, -0.7356480,
     -0.10189282, 0.29788946, 0.2020574),
    (-0.50329486, -0.33273860, 0.67548709, -0.4239072),
    (0.2581,),
    (-0.1005, 0.2981, 0.2029)])
  new_qd = [
    body.joint.time_step_velocity(qd=body.qd, qdd=qdd_i, delta_t=0.01)
      for body,qdd_i in zip(model.bodies, qdd)]
  assert approx_equal(new_qd, [
    (0.1796, -0.0195, -0.1577, -0.0501, -0.1908, 0.2904),
    (0.1208, -0.0808, 0.1099),
    (-0.1886,),
    (-0.0501, -0.1934, 0.2928)])
  for body,q in zip(model.bodies, [(1,2,3,4,5,6,7),(8,9,10,11),(12,)]):
    assert approx_equal(body.joint.new_q(q=q).get_q(), q)
  #
  qdd = []
  for body in model.bodies:
    body.qd = body.joint.qd_zero
    qdd.append(body.joint.qdd_zero)
  model.flag_velocities_as_changed()
  tau = model.inverse_dynamics(qdd_array=qdd, f_ext_array=f_ext)
  assert approx_equal(tau, [
    (0.286606889188, -0.220046556736, 1.14581449056,
     0.153659017347, -0.0162376124727, -0.155270178613),
    (0.541174667239, 0.406026437633, -0.813718623664),
    (0.07,),
    (-0.35,0.02,0.2)])
  tau0 = model.f_ext_as_tau(f_ext_array=f_ext)
  assert approx_equal(tau0, tau)