Ejemplo n.º 1
0
    def _get_C(self, i_X_p, Si, Ic, q, q_dot, n_joints,
               gravity=None, f_ext=None):
        """Internal function for calculating the joint space bias matrix."""

        v = []
        a = []
        f = []
        C = cs.SX.zeros(n_joints)

        for i in range(0, n_joints):
            vJ = cs.mtimes(Si[i], q_dot[i])
            if i == 0:
                v.append(vJ)
                if gravity is not None:
                    ag = np.array([0., 0., 0., gravity[0], gravity[1], gravity[2]])
                    a.append(cs.mtimes(i_X_p[i], -ag))
                else:
                    a.append(cs.SX([0., 0., 0., 0., 0., 0.]))
            else:
                v.append(cs.mtimes(i_X_p[i], v[i-1]) + vJ)
                a.append(cs.mtimes(i_X_p[i], a[i-1]) + cs.mtimes(plucker.motion_cross_product(v[i]),vJ))

            f.append(cs.mtimes(Ic[i], a[i]) + cs.mtimes(plucker.force_cross_product(v[i]), cs.mtimes(Ic[i], v[i])))

        if f_ext is not None:
            f = self._apply_external_forces(f_ext, f, i_X_0)

        for i in range(n_joints-1, -1, -1):
            C[i] = cs.mtimes(Si[i].T, f[i])
            if i != 0:
                f[i-1] = f[i-1] + cs.mtimes(i_X_p[i].T, f[i])

        return C
Ejemplo n.º 2
0
    def get_inverse_dynamics_rnea(self, root, tip,
                                  gravity=None, f_ext=None):
        """Returns the inverse dynamics as a casadi function."""
        if self.robot_desc is None:
            raise ValueError('Robot description not loaded from urdf')

        n_joints = self.get_n_joints(root, tip)
        q = cs.SX.sym("q", n_joints)
        q_dot = cs.SX.sym("q_dot", n_joints)
        q_ddot = cs.SX.sym("q_ddot", n_joints)
        i_X_p, Si, Ic = self._model_calculation(root, tip, q)

        v = []
        a = []
        f = []
        tau = cs.SX.zeros(n_joints)

        for i in range(0, n_joints):
            vJ = cs.mtimes(Si[i], q_dot[i])
            if i == 0:
                v.append(vJ)
                if gravity is not None:
                    ag = np.array([0.,
                                   0.,
                                   0.,
                                   gravity[0],
                                   gravity[1],
                                   gravity[2]])
                    a.append(
                        cs.mtimes(i_X_p[i], -ag) + cs.mtimes(Si[i], q_ddot[i]))
                else:
                    a.append(cs.mtimes(Si[i], q_ddot[i]))
            else:
                v.append(cs.mtimes(i_X_p[i], v[i-1]) + vJ)
                a.append(
                    cs.mtimes(i_X_p[i], a[i-1])
                    + cs.mtimes(Si[i], q_ddot[i])
                    + cs.mtimes(plucker.motion_cross_product(v[i]), vJ))

            f.append(
                cs.mtimes(Ic[i], a[i])
                + cs.mtimes(
                    plucker.force_cross_product(v[i]),
                    cs.mtimes(Ic[i], v[i])))

        if f_ext is not None:
            f = self._apply_external_forces(f_ext, f, i_X_p)

        for i in range(n_joints-1, -1, -1):
            tau[i] = cs.mtimes(Si[i].T, f[i])
            if i != 0:
                f[i-1] = f[i-1] + cs.mtimes(i_X_p[i].T, f[i])

        tau = cs.Function("C", [q, q_dot, q_ddot], [tau], self.func_opts)
        return tau
Ejemplo n.º 3
0
    def get_forward_dynamics_aba(self, root, tip, gravity=None, f_ext=None):
        """Returns the forward dynamics as a casadi function using the
        articulated body algorithm."""

        if self.robot_desc is None:
            raise ValueError('Robot description not loaded from urdf')

        n_joints = self.get_n_joints(root, tip)
        q = cs.SX.sym("q", n_joints)
        q_dot = cs.SX.sym("q_dot", n_joints)
        tau = cs.SX.sym("tau", n_joints)
        q_ddot = cs.SX.zeros(n_joints)
        i_X_p, Si, Ic = self._model_calculation(root, tip, q)

        v = []
        c = []
        pA = []
        IA = []

        u = [None] * n_joints
        U = [None] * n_joints
        d = [None] * n_joints

        for i in range(0, n_joints):
            vJ = cs.mtimes(Si[i], q_dot[i])
            if i == 0:
                v.append(vJ)
                c.append([0, 0, 0, 0, 0, 0])
            else:
                v.append(cs.mtimes(i_X_p[i], v[i - 1]) + vJ)
                c.append(cs.mtimes(plucker.motion_cross_product(v[i]), vJ))
            IA.append(Ic[i])
            pA.append(
                cs.mtimes(plucker.force_cross_product(v[i]),
                          cs.mtimes(Ic[i], v[i])))

        if f_ext is not None:
            pA = self._apply_external_forces(f_ext, pA)

        for i in range(n_joints - 1, -1, -1):
            U[i] = cs.mtimes(IA[i], Si[i])
            d[i] = cs.mtimes(Si[i].T, U[i])
            u[i] = tau[i] - cs.mtimes(Si[i].T, pA[i])
            if i != 0:
                Ia = IA[i] - ((cs.mtimes(U[i], U[i].T) / d[i]))
                pa = pA[i] + cs.mtimes(Ia,
                                       c[i]) + (cs.mtimes(U[i], u[i]) / d[i])
                IA[i - 1] += cs.mtimes(i_X_p[i].T, cs.mtimes(Ia, i_X_p[i]))
                pA[i - 1] += cs.mtimes(i_X_p[i].T, pa)

        a = []
        for i in range(0, n_joints):
            if i == 0:
                if gravity is not None:
                    ag = np.array(
                        [0., 0., 0., gravity[0], gravity[1], gravity[2]])
                    a_temp = (cs.mtimes(i_X_p[i], -ag) + c[i])
                else:
                    a_temp = c[i]
            else:
                a_temp = (cs.mtimes(i_X_p[i], a[i - 1]) + c[i])
            q_ddot[i] = (u[i] - cs.mtimes(U[i].T, a_temp)) / d[i]
            a.append(a_temp + cs.mtimes(Si[i], q_ddot[i]))

        q_ddot = cs.Function("q_ddot", [q, q_dot, tau], [q_ddot],
                             self.func_opts)
        return q_ddot