Ejemplos de jacob0 en Python

Ejemplo n.º 1

Archivo: ospace.py Proyecto: BrendanBenshoof/myro-epuck

def ospace(robot, q, qd):
    q = mat(q)
    qd = mat(qd)
    M = inertia(robot, q)
    C = coriolis(robot, q, qd)
    g = gravload(robot, q)
    J = jacob0(robot, q)
    Ji = inv(J)
    print 'Ji\n',Ji,'\n\n'
    print 'J\n',J,'\n\n'
    print 'M\n',M,'\n\n'
    print 'C\n',C,'\n\n'
    print 'g\n',g,'\n\n'
    Lambda = Ji.T*M*Ji
    mu = J.T*C - Lamba*H*qd
    p = J.T*g
    return Lambda, mu, p

Ejemplo n.º 2

Archivo: ospace.py Proyecto: yingted/Myro

def ospace(robot, q, qd):
    q = mat(q)
    qd = mat(qd)
    M = inertia(robot, q)
    C = coriolis(robot, q, qd)
    g = gravload(robot, q)
    J = jacob0(robot, q)
    Ji = inv(J)
    print 'Ji\n', Ji, '\n\n'
    print 'J\n', J, '\n\n'
    print 'M\n', M, '\n\n'
    print 'C\n', C, '\n\n'
    print 'g\n', g, '\n\n'
    Lambda = Ji.T * M * Ji
    mu = J.T * C - Lamba * H * qd
    p = J.T * g
    return Lambda, mu, p

Ejemplo n.º 3

Archivo: kinematics_test.py Proyecto: Samuel-Drew/Sams-Test-Repo

print 'fkine(stanf,q5)\n'
print fkine(stanf, q5), '\n\n'
print 'fkine(stanf,[1,2,1,0,2,3])\n'
print fkine(stanf, [1, 2, 1, 0, 2, 3]), '\n\n'
print '\t\t\t***Test for Jacobian***\n'
print 'J1 = jacobn(stanf,[1,2,1,0,2,3])\n'
J1 = jacobn(stanf, [1, 2, 1, 0, 2, 3])
print J1, '\n\n'
print 'J2 = jacobn(p560m,[1,0,3,-3,0,1])\n'
J2 = jacobn(p560m, [1, 0, 3, -3, 0, 1])
print J2, '\n\n'
print 'J3 = jacobn(p560,[1,0,3,-3,0,1])\n'
J3 = jacobn(p560, [1, 0, 3, -3, 0, 1])
print J3, '\n\n'
print 'J01 = jacob0(stanf,[1,2,1,0,2,3])\n'
J01 = jacob0(stanf, [1, 2, 1, 0, 2, 3])
print J01, '\n\n'
print 'J02 = jacob0(p560m,[1,0,3,-3,0,1])\n'
J02 = jacob0(p560m, [1, 0, 3, -3, 0, 1])
print J02, '\n\n'
print 'J03 = jacob0(p560,[1,0,3,-3,0,1])\n'
J03 = jacob0(p560, [1, 0, 3, -3, 0, 1])
print J03, '\n\n'
print '\t\t\t***Trajectory Test***\n'
print 'Joint trajectory'
print 'qj,qdj,qddj = jtraj([0,0,0,0,0,0],[pi/2, pi/4, -3*pi/5, 4*pi/6, 0, 1], 5)\n'
qj, qdj, qddj = jtraj([0, 0, 0, 0, 0, 0],
                      [pi / 2, pi / 4, -3 * pi / 5, 4 * pi / 6, 0, 1], 5)
print 'qj:\n', qj, '\n\nqdj:\n', qdj, '\n\nqddj:\n', qddj, '\n\n'
print 'Cartesian trajectory'
print 'tt = ctraj(fkine(p560m,[0,0,0,0,0,0]), fkine(p560m,[pi/2,pi/4,-3*pi/4,-pi/8,0,1]), 5)\n'

Ejemplo n.º 4

Archivo: kinematics_test.py Proyecto: JoseEduinCulmaCaviedes/robotics-toolbox-python

print 'fkine(stanf,q5)\n'
print fkine(stanf,q5),'\n\n'
print 'fkine(stanf,[1,2,1,0,2,3])\n'
print fkine(stanf,[1,2,1,0,2,3]),'\n\n'
print '\t\t\t***Test for Jacobian***\n'
print 'J1 = jacobn(stanf,[1,2,1,0,2,3])\n'
J1 = jacobn(stanf,[1,2,1,0,2,3])
print J1,'\n\n'
print 'J2 = jacobn(p560m,[1,0,3,-3,0,1])\n'
J2 = jacobn(p560m,[1,0,3,-3,0,1])
print J2,'\n\n'
print 'J3 = jacobn(p560,[1,0,3,-3,0,1])\n'
J3 = jacobn(p560,[1,0,3,-3,0,1])
print J3,'\n\n'
print 'J01 = jacob0(stanf,[1,2,1,0,2,3])\n'
J01 = jacob0(stanf,[1,2,1,0,2,3])
print J01,'\n\n'
print 'J02 = jacob0(p560m,[1,0,3,-3,0,1])\n'
J02 = jacob0(p560m,[1,0,3,-3,0,1])
print J02,'\n\n'
print 'J03 = jacob0(p560,[1,0,3,-3,0,1])\n'
J03 = jacob0(p560,[1,0,3,-3,0,1])
print J03,'\n\n'
print '\t\t\t***Trajectory Test***\n'
print 'Joint trajectory'
print 'qj,qdj,qddj = jtraj([0,0,0,0,0,0],[pi/2, pi/4, -3*pi/5, 4*pi/6, 0, 1], 5)\n'
qj,qdj,qddj = jtraj([0,0,0,0,0,0],[pi/2, pi/4, -3*pi/5, 4*pi/6, 0, 1], 5)
print 'qj:\n',qj,'\n\nqdj:\n',qdj,'\n\nqddj:\n',qddj,'\n\n'
print 'Cartesian trajectory'
print 'tt = ctraj(fkine(p560m,[0,0,0,0,0,0]), fkine(p560m,[pi/2,pi/4,-3*pi/4,-pi/8,0,1]), 5)\n'
tt = ctraj(fkine(p560m,[0,0,0,0,0,0]), fkine(p560m,[pi/2,pi/4,-3*pi/4,-pi/8,0,1]), 5)