示例#1
0
def exercise_basic():
    assert approx_equal(sum(spatial_lib.xrot((1, 2, 3, 4, 5, 6, 7, 8, 9))), 90)
    assert approx_equal(sum(spatial_lib.xtrans((1, 2, 3))), 6)
    assert approx_equal(
        sum(
            spatial_lib.cb_as_spatial_transform(cb=matrix.rt(((1, 2, 3, 4, 5,
                                                               6, 7, 8, 9),
                                                              (1, 2, 3))))),
        90)
    assert approx_equal(sum(spatial_lib.crm((1, 2, 3, 4, 5, 6))), 0)
    assert approx_equal(sum(spatial_lib.crf((1, 2, 3, 4, 5, 6))), 0)
    i_spatial = spatial_lib.mci(m=1.234,
                                c=matrix.col((1, 2, 3)),
                                i=matrix.sym(sym_mat3=(2, 3, 4, 0.1, 0.2,
                                                       0.3)))
    assert approx_equal(sum(i_spatial), 21.306)
    assert approx_equal(
        spatial_lib.kinetic_energy(i_spatial=i_spatial,
                                   v_spatial=matrix.col((1, 2, 3, 4, 5, 6))),
        75.109)
    #
    mass_points = body_lib.mass_points(sites=matrix.col_list([
        (0.949, 2.815, 5.189), (0.405, 3.954, 5.917), (0.779, 5.262, 5.227)
    ]),
                                       masses=[2.34, 3.56, 1.58])
    assert approx_equal(mass_points.sum_of_masses(), 7.48)
    assert approx_equal(mass_points.center_of_mass(),
                        [0.654181818182, 3.87397058824, 5.54350802139])
    assert approx_equal(mass_points._sum_of_masses, 7.48)
    assert approx_equal(
        mass_points.inertia(pivot=matrix.col((0.9, -1.3, 0.4))), [
            404.7677928, 10.04129606, 10.09577652, 10.04129606, 199.7384559,
            -199.3511949, 10.09577652, -199.3511949, 206.8314171
        ])
示例#2
0
def exercise_basic():
  assert approx_equal(sum(spatial_lib.xrot((1,2,3,4,5,6,7,8,9))), 90)
  assert approx_equal(sum(spatial_lib.xtrans((1,2,3))), 6)
  assert approx_equal(
    sum(spatial_lib.cb_as_spatial_transform(
      cb=matrix.rt(((1,2,3,4,5,6,7,8,9), (1,2,3))))), 90)
  assert approx_equal(sum(spatial_lib.crm((1,2,3,4,5,6))), 0)
  assert approx_equal(sum(spatial_lib.crf((1,2,3,4,5,6))), 0)
  i_spatial = spatial_lib.mci(
    m=1.234,
    c=matrix.col((1,2,3)),
    i=matrix.sym(sym_mat3=(2,3,4,0.1,0.2,0.3)))
  assert approx_equal(sum(i_spatial), 21.306)
  assert approx_equal(spatial_lib.kinetic_energy(
    i_spatial=i_spatial, v_spatial=matrix.col((1,2,3,4,5,6))), 75.109)
  #
  mass_points = body_lib.mass_points(
    sites=matrix.col_list([
      (0.949, 2.815, 5.189),
      (0.405, 3.954, 5.917),
      (0.779, 5.262, 5.227)]),
    masses=[2.34, 3.56, 1.58])
  assert approx_equal(mass_points.sum_of_masses(), 7.48)
  assert approx_equal(mass_points.center_of_mass(),
    [0.654181818182, 3.87397058824, 5.54350802139])
  assert approx_equal(mass_points._sum_of_masses, 7.48)
  assert approx_equal(
    mass_points.inertia(pivot=matrix.col((0.9,-1.3,0.4))),
    [404.7677928, 10.04129606, 10.09577652,
     10.04129606, 199.7384559, -199.3511949,
     10.09577652, -199.3511949, 206.8314171])
    def inverse_dynamics(O, qdd_array=None, f_ext_array=None, grav_accn=None):
        """RBDA Tab. 5.1, p. 96:
Inverse Dynamics of a kinematic tree via Recursive Newton-Euler Algorithm.
qdd_array is a vector of joint acceleration variables.
The return value (tau) is a vector of joint force variables.
f_ext_array specifies external forces acting on the bodies. If f_ext_array
is None then there are no external forces; otherwise, f_ext[i] is a spatial
force vector giving the force acting on body i, expressed in body i
coordinates.
grav_accn is a 6D vector expressing the linear acceleration due to gravity.
    """
        assert qdd_array is None or len(qdd_array) == len(O.bodies)
        assert f_ext_array is None or len(f_ext_array) == len(O.bodies)
        nb = len(O.bodies)
        xup_array = O.xup_array()
        v = O.spatial_velocities()
        a = [None] * nb
        f = [None] * nb
        for ib in xrange(nb):
            body = O.bodies[ib]
            s = body.joint.motion_subspace
            if (s is None):
                vj = body.qd
                if (qdd_array is None or qdd_array[ib] is None):
                    aj = matrix.zeros(n=6)
                else:
                    aj = qdd_array[ib]
            else:
                vj = s * body.qd
                if (qdd_array is None or qdd_array[ib] is None):
                    aj = matrix.zeros(n=6)
                else:
                    aj = s * qdd_array[ib]
            if (body.parent == -1):
                a[ib] = aj
                if (grav_accn is not None):
                    a[ib] += xup_array[ib] * (-grav_accn)
            else:
                a[ib] = xup_array[ib] * a[body.parent] + aj + crm(v[ib]) * vj
            f[ib] = body.i_spatial * a[ib] + crf(
                v[ib]) * (body.i_spatial * v[ib])
            if (f_ext_array is not None and f_ext_array[ib] is not None):
                f[ib] -= f_ext_array[ib]
        #
        tau_array = [None] * nb
        for ib in xrange(nb - 1, -1, -1):
            body = O.bodies[ib]
            s = body.joint.motion_subspace
            if (s is None):
                tau_array[ib] = f[ib]
            else:
                tau_array[ib] = s.transpose() * f[ib]
            if (body.parent != -1):
                f[body.parent] += xup_array[ib].transpose() * f[ib]
        #
        return tau_array
示例#4
0
  def inverse_dynamics(O, qdd_array=None, f_ext_array=None, grav_accn=None):
    """RBDA Tab. 5.1, p. 96:
Inverse Dynamics of a kinematic tree via Recursive Newton-Euler Algorithm.
qdd_array is a vector of joint acceleration variables.
The return value (tau) is a vector of joint force variables.
f_ext_array specifies external forces acting on the bodies. If f_ext_array
is None then there are no external forces; otherwise, f_ext[i] is a spatial
force vector giving the force acting on body i, expressed in body i
coordinates.
grav_accn is a 6D vector expressing the linear acceleration due to gravity.
    """
    assert qdd_array is None or len(qdd_array) == len(O.bodies)
    assert f_ext_array is None or len(f_ext_array) == len(O.bodies)
    nb = len(O.bodies)
    xup_array = O.xup_array()
    v = O.spatial_velocities()
    a = [None] * nb
    f = [None] * nb
    for ib in xrange(nb):
      body = O.bodies[ib]
      s = body.joint.motion_subspace
      if (s is None):
        vj = body.qd
        if (qdd_array is None or qdd_array[ib] is None):
          aj = matrix.zeros(n=6)
        else:
          aj = qdd_array[ib]
      else:
        vj = s * body.qd
        if (qdd_array is None or qdd_array[ib] is None):
          aj = matrix.zeros(n=6)
        else:
          aj = s * qdd_array[ib]
      if (body.parent == -1):
        a[ib] = aj
        if (grav_accn is not None):
          a[ib] += xup_array[ib] * (-grav_accn)
      else:
        a[ib] = xup_array[ib] * a[body.parent] + aj + crm(v[ib]) * vj
      f[ib] = body.i_spatial * a[ib] + crf(v[ib]) * (body.i_spatial * v[ib])
      if (f_ext_array is not None and f_ext_array[ib] is not None):
        f[ib] -= f_ext_array[ib]
    #
    tau_array = [None] * nb
    for ib in xrange(nb-1,-1,-1):
      body = O.bodies[ib]
      s = body.joint.motion_subspace
      if (s is None):
        tau_array[ib] = f[ib]
      else:
        tau_array[ib] = s.transpose() * f[ib]
      if (body.parent != -1):
        f[body.parent] += xup_array[ib].transpose() * f[ib]
    #
    return tau_array
示例#5
0
  def forward_dynamics_ab(O, tau_array=None, f_ext_array=None, grav_accn=None):
    """RBDA Tab. 7.1, p. 132:
Forward Dynamics of a kinematic tree via the Articulated-Body Algorithm.
tau_array is a vector of force variables.
The return value (qdd_array) is a vector of joint acceleration variables.
f_ext_array specifies external forces acting on the bodies. If f_ext_array
is None then there are no external forces; otherwise, f_ext_array[i] is a
spatial force vector giving the force acting on body i, expressed in body i
coordinates.
grav_accn is a 6D vector expressing the linear acceleration due to gravity.
    """
    assert tau_array is None or len(tau_array) == len(O.bodies)
    assert f_ext_array is None or len(f_ext_array) == len(O.bodies)
    nb = len(O.bodies)
    xup_array = O.xup_array()
    v = O.spatial_velocities()
    c = [None] * nb
    ia = [None] * nb
    pa = [None] * nb
    for ib in xrange(nb):
      body = O.bodies[ib]
      s = body.joint.motion_subspace
      if (s is None): vj = body.qd
      else:           vj = s * body.qd
      if (body.parent == -1): c[ib] = matrix.col([0,0,0,0,0,0])
      else:                   c[ib] = crm(v[ib]) * vj
      ia[ib] = body.i_spatial
      pa[ib] = crf(v[ib]) * (body.i_spatial * v[ib])
      if (f_ext_array is not None and f_ext_array[ib] is not None):
        pa[ib] -= f_ext_array[ib]
    #
    u = [None] * nb
    d_inv = [None] * nb
    u_ = [None] * nb
    for ib in xrange(nb-1,-1,-1):
      body = O.bodies[ib]
      s = body.joint.motion_subspace
      if (s is None):
        u[ib] = ia[ib]
        d = u[ib]
        u_[ib] = -pa[ib]
      else:
        u[ib] = ia[ib] * s
        d = s.transpose() * u[ib]
        u_[ib] = -s.transpose() * pa[ib]
      if (tau_array is not None and tau_array[ib] is not None):
        u_[ib] += tau_array[ib]
      d_inv[ib] = generalized_inverse(d)
      if (body.parent != -1):
        u_d_inv = u[ib] * d_inv[ib];
        ia_ = ia[ib] - u_d_inv * u[ib].transpose()
        pa_ = pa[ib] + ia_*c[ib] + u_d_inv * u_[ib]
        ia[body.parent] += xup_array[ib].transpose() * ia_ * xup_array[ib]
        pa[body.parent] += xup_array[ib].transpose() * pa_
    #
    a = [None] * nb
    qdd_array = [None] * nb
    for ib in xrange(nb):
      body = O.bodies[ib]
      s = body.joint.motion_subspace
      if (body.parent == -1):
        a[ib] = c[ib]
        if (grav_accn is not None):
          a[ib] += xup_array[ib] * (-grav_accn)
      else:
        a[ib] = xup_array[ib] * a[body.parent] + c[ib]
      qdd_array[ib] = d_inv[ib] * (u_[ib] - u[ib].transpose()*a[ib])
      if (s is None):
        a[ib] += qdd_array[ib]
      else:
        a[ib] += s * qdd_array[ib]
    #
    return qdd_array
    def forward_dynamics_ab(O,
                            tau_array=None,
                            f_ext_array=None,
                            grav_accn=None):
        """RBDA Tab. 7.1, p. 132:
Forward Dynamics of a kinematic tree via the Articulated-Body Algorithm.
tau_array is a vector of force variables.
The return value (qdd_array) is a vector of joint acceleration variables.
f_ext_array specifies external forces acting on the bodies. If f_ext_array
is None then there are no external forces; otherwise, f_ext_array[i] is a
spatial force vector giving the force acting on body i, expressed in body i
coordinates.
grav_accn is a 6D vector expressing the linear acceleration due to gravity.
    """
        assert tau_array is None or len(tau_array) == len(O.bodies)
        assert f_ext_array is None or len(f_ext_array) == len(O.bodies)
        nb = len(O.bodies)
        xup_array = O.xup_array()
        v = O.spatial_velocities()
        c = [None] * nb
        ia = [None] * nb
        pa = [None] * nb
        for ib in xrange(nb):
            body = O.bodies[ib]
            s = body.joint.motion_subspace
            if (s is None): vj = body.qd
            else: vj = s * body.qd
            if (body.parent == -1): c[ib] = matrix.col([0, 0, 0, 0, 0, 0])
            else: c[ib] = crm(v[ib]) * vj
            ia[ib] = body.i_spatial
            pa[ib] = crf(v[ib]) * (body.i_spatial * v[ib])
            if (f_ext_array is not None and f_ext_array[ib] is not None):
                pa[ib] -= f_ext_array[ib]
        #
        u = [None] * nb
        d_inv = [None] * nb
        u_ = [None] * nb
        for ib in xrange(nb - 1, -1, -1):
            body = O.bodies[ib]
            s = body.joint.motion_subspace
            if (s is None):
                u[ib] = ia[ib]
                d = u[ib]
                u_[ib] = -pa[ib]
            else:
                u[ib] = ia[ib] * s
                d = s.transpose() * u[ib]
                u_[ib] = -s.transpose() * pa[ib]
            if (tau_array is not None and tau_array[ib] is not None):
                u_[ib] += tau_array[ib]
            d_inv[ib] = generalized_inverse(d)
            if (body.parent != -1):
                u_d_inv = u[ib] * d_inv[ib]
                ia_ = ia[ib] - u_d_inv * u[ib].transpose()
                pa_ = pa[ib] + ia_ * c[ib] + u_d_inv * u_[ib]
                ia[body.
                   parent] += xup_array[ib].transpose() * ia_ * xup_array[ib]
                pa[body.parent] += xup_array[ib].transpose() * pa_
        #
        a = [None] * nb
        qdd_array = [None] * nb
        for ib in xrange(nb):
            body = O.bodies[ib]
            s = body.joint.motion_subspace
            if (body.parent == -1):
                a[ib] = c[ib]
                if (grav_accn is not None):
                    a[ib] += xup_array[ib] * (-grav_accn)
            else:
                a[ib] = xup_array[ib] * a[body.parent] + c[ib]
            qdd_array[ib] = d_inv[ib] * (u_[ib] - u[ib].transpose() * a[ib])
            if (s is None):
                a[ib] += qdd_array[ib]
            else:
                a[ib] += s * qdd_array[ib]
        #
        return qdd_array