Python jointJacobianの例

コード例 #1

    def jacobian(self, c2, robot, q):
        WORLD = se3.ReferenceFrame.WORLD
        Ja = se3.jointJacobian(robot.model, robot.data, q, self.jointParent,
                               WORLD, True)
        Jb = se3.jointJacobian(robot.model, robot.data, q, c2.jointParent,
                               WORLD, True)

        Xa = se3.SE3(self.R, self.w).action
        Xb = se3.SE3(c2.R, c2.w).action

        J = (Xa * Ja)[2, :] - (Xb * Jb)[2, :]
        return J

コード例 #2

ファイル: bindings_dynamics.py プロジェクト: nim65s/pinocchio

    def setUp(self):
        self.model = pin.buildSampleModelHumanoidRandom()
        self.data = self.model.createData()

        qmax = np.matrix(np.full((self.model.nq,1),np.pi))
        self.q = pin.randomConfiguration(self.model,-qmax,qmax)
        self.v = rand(self.model.nv)
        self.tau = rand(self.model.nv)

        self.v0 = zero(self.model.nv)
        self.tau0 = zero(self.model.nv)
        self.tolerance = 1e-9

        # we compute J on a different self.data
        self.J = pin.jointJacobian(self.model,self.model.createData(),self.q,self.model.getJointId('lleg6_joint'))
        self.gamma = zero(6)

コード例 #3

ファイル: bindings_dynamics.py プロジェクト: zhilinxiong/pinocchio

    def setUp(self):
        self.model = pin.buildSampleModelHumanoidRandom()
        self.data = self.model.createData()

        qmax = np.matrix(np.full((self.model.nq, 1), np.pi))
        self.q = pin.randomConfiguration(self.model, -qmax, qmax)
        self.v = rand(self.model.nv)
        self.tau = rand(self.model.nv)

        self.v0 = zero(self.model.nv)
        self.tau0 = zero(self.model.nv)
        self.tolerance = 1e-9

        # we compute J on a different self.data
        self.J = pin.jointJacobian(self.model, self.model.createData(), self.q,
                                   self.model.getJointId('lleg6_joint'))
        self.gamma = zero(6)

コード例 #4

ファイル: i-inverse-kinematics.py プロジェクト: jkw0701/pinocchio

data = model.createData()

JOINT_ID = 6
xdes = np.matrix([0.5, -0.5, 0.5]).T

q = model.neutralConfiguration
eps = 1e-4
IT_MAX = 1000
DT = 1e-1

for i in range(IT_MAX):
    pinocchio.forwardKinematics(model, data, q)
    x = data.oMi[JOINT_ID].translation
    R = data.oMi[JOINT_ID].rotation
    err = R.T * (x - xdes)
    if np.linalg.norm(err) < eps:
        print("Convergence achieved!")
        break
    J = pinocchio.jointJacobian(model, data, q, JOINT_ID,
                                pinocchio.ReferenceFrame.LOCAL, True)[:3, :]
    v = -np.linalg.pinv(J) * err
    q = pinocchio.integrate(model, q, v * DT)
    if not i % 10: print('error = %s' % (x - xdes).T)
else:
    print(
        "\nWarning: the iterative algorithm has not reached convergence to the desired precision"
    )

print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % (x - xdes).T)

コード例 #5

ファイル: i-inverse-kinematics.py プロジェクト: zhilinxiong/pinocchio

JOINT_ID = 6
xdes = np.matrix([0.5, -0.5, 0.5]).T

q = pinocchio.neutral(model)
eps = 1e-4
IT_MAX = 1000
DT = 1e-1

i = 0
while True:
    pinocchio.forwardKinematics(model, data, q)
    x = data.oMi[JOINT_ID].translation
    R = data.oMi[JOINT_ID].rotation
    err = R.T * (x - xdes)
    if np.linalg.norm(err) < eps:
        print("Convergence achieved!")
        break
    if i >= IT_MAX:
        print(
            "\nWarning: the iterative algorithm has not reached convergence to the desired precision"
        )
        break
    J = pinocchio.jointJacobian(model, data, q, JOINT_ID)[:3, :]
    v = -np.linalg.pinv(J) * err
    q = pinocchio.integrate(model, q, v * DT)
    if not i % 10: print('error = %s' % err.T)
    i += 1

print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % err.T)

コード例 #6

ファイル: i-inverse-kinematics.py プロジェクト: nim65s/pinocchio

import pinocchio

model = pinocchio.buildSampleModelManipulator()
data  = model.createData()

JOINT_ID = 6
xdes     = np.matrix([ 0.5,-0.5,0.5]).T

q        = model.neutralConfiguration
eps      = 1e-4
IT_MAX   = 1000
DT       = 1e-1

for i in range(IT_MAX):
    pinocchio.forwardKinematics(model,data,q)
    x   = data.oMi[JOINT_ID].translation
    R   = data.oMi[JOINT_ID].rotation
    err = R.T*(x-xdes)
    if np.linalg.norm(err) < eps:
        print("Convergence achieved!")
        break
    J   = pinocchio.jointJacobian(model,data,q,JOINT_ID,pinocchio.ReferenceFrame.LOCAL,True)[:3,:]
    v   = - np.linalg.pinv(J)*err
    q   = pinocchio.integrate(model,q,v*DT)
    if not i % 10:        print('error = %s' % (x-xdes).T)
else:
    print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")

print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % (x-xdes).T)

コード例 #7

ファイル: i-inverse-kinematics.py プロジェクト: stack-of-tasks/pinocchio

data  = model.createData()

JOINT_ID = 6
xdes     = np.matrix([ 0.5,-0.5,0.5]).T

q        = pinocchio.neutral(model)
eps      = 1e-4
IT_MAX   = 1000
DT       = 1e-1

i=0
while True:
    pinocchio.forwardKinematics(model,data,q)
    x   = data.oMi[JOINT_ID].translation
    R   = data.oMi[JOINT_ID].rotation
    err = R.T*(x-xdes)
    if np.linalg.norm(err) < eps:
        print("Convergence achieved!")
        break
    if i >= IT_MAX:
        print("\nWarning: the iterative algorithm has not reached convergence to the desired precision")
        break
    J   = pinocchio.jointJacobian(model,data,q,JOINT_ID)[:3,:]
    v   = - np.linalg.pinv(J)*err
    q   = pinocchio.integrate(model,q,v*DT)
    if not i % 10:        print('error = %s' % err.T)
    i += 1

print('\nresult: %s' % q.flatten().tolist())
print('\nfinal error: %s' % err.T)