Some problem in the pyrobolearn/kinematics/inverse/moving_sphere.py

[GePro-hit]

I have try the track the orientation of the target sphere not just the position.
dv = kp_x * (sphere.position - x) + kd_x * dx I try to use this function to get the position control. It has a good performance. but when I add the orientation tracking using dw=kp_o* error_quaternion(sphere.orientation, quaternion_endeffector) + kd_o * do_d[0:3]
dq = Jp.dot(np.hstack((dv, dw))) but the performance of the tracking is not good.
the error_quaternion is
def error_quaternion(a, b):
q = a.copy()
p = b.copy()
x = q[0] * p[1] - q[1] * p[0] - q[2] * p[3] + q[3] * p[2]
y = q[0] * p[2] + q[1] * p[3] - q[2] * p[0] - q[3] * p[1]
z = q[0] * p[3] - q[1] * p[2] + q[2] * p[1] - q[3] * p[0]
return np.array([x, y, z])
do is error_quaternion_q(quaternion_endeffector, sphere.orientation) / dt
error_quaternion_q is
def error_quaternion_q(a, b):
q = a.copy()
p = b.copy()
w = q[0] * p[0] - q[1] * p[1] - q[2] * p[2] - q[3] * p[3]
x = q[0] * p[1] - q[1] * p[0] - q[2] * p[3] + q[3] * p[2]
y = q[0] * p[2] + q[1] * p[3] - q[2] * p[0] - q[3] * p[1]
z = q[0] * p[3] - q[1] * p[2] + q[2] * p[1] - q[3] * p[0]
return np.array([x, y, z, w])

[GePro-hit]

the red line is the desired trajectory and the blue line is the actual end-effector pose

[bdelhaisse]

Hi,

I am pretty busy this week, and will have a look at your problem later. In the meantime, did you have a look at the quaternion_error function defined here:

pyrobolearn/pyrobolearn/utils/transformation.py

Line 989 in b1f4ac8

def quaternion_error(quat_des, quat_cur):

You could first compare if your function error_quaternion returns the same thing as that function.

Also, there is another example in the folder priorities (https://github.com/robotlearn/pyrobolearn/tree/master/pyrobolearn/priorities) which uses Quadratic Programming to solve the inverse kinematics (IK) problem: https://github.com/robotlearn/pyrobolearn/blob/master/pyrobolearn/priorities/ik.py. In that one, if I remember correctly, I could track the orientation as well. You could have a look at this file and uncomment the following line:

pyrobolearn/pyrobolearn/priorities/ik.py

Line 38 in b1f4ac8

# desired_orientation=quat_des, kp_orientation=50.)

I will soon clean that file ik.py and put it in the examples folder.

Best,
Brian

[GePro-hit]

@bdelhaisse Thanks for your immediately answer, It seems like the function I have written have some mistakes. By the way, when i run the pyrobolearn/pyrobolearn/priorities/ik.py without any changes it push the error:
Traceback (most recent call last):
File "/home/tiboy/simulation/pyrobolearn/pyrobolearn/priorities/ik.py", line 63, in
dq = solver.solve()
File "/home/tiboy/simulation/pyrobolearn/pyrobolearn/priorities/solvers/qp_task_solver.py", line 228, in solve
x_opt = self.solver.optimize(Q=Q, p=p, x0=x_opt, G=G, h=h, A=F, b=k)
File "/home/tiboy/simulation/pyrobolearn/pyrobolearn/optimizers/qpsolvers_optimizer.py", line 178, in optimize
return qpsolvers.solve_qp(Q, p, G, h, A, b, solver=self.method, initvals=x0, sym_proj=self.sym_proj)
File "/home/tiboy/py3.5/lib/python3.5/site-packages/qpsolvers/init.py", line 180, in solve_qp
return quadprog_solve_qp(P, q, G, h, A, b, initvals=initvals)
File "/home/tiboy/py3.5/lib/python3.5/site-packages/qpsolvers/quadprog_.py", line 81, in quadprog_solve_qp
return solve_qp(qp_G, qp_a, qp_C, qp_b, meq)[0]
File "quadprog/quadprog.pyx", line 102, in quadprog.solve_qp
ValueError: constraints are inconsistent, no solution

[bdelhaisse]

Hi,

I just launched the ik.py file and I don't get the error that you have. It seems to be an error with qpsolvers. Which version of the qpsolvers library do you have? On my system, I have qpsolvers==1.0.4. Also, if I recall correctly, I submitted a pull request few months ago (which was accepted). So, there are two possibilities:

1. the version of the library you have is old and you need to update it.
2. the version of the library is higher than mine and the author modified something in his code (see https://github.com/stephane-caron/qpsolvers)

If I were you, I would try to install it locally by cloning the repository, and see if it works or if you still have the error.

The function quadprog_solve_qp that I have in the file quadprog_.py is (without the doc):

def quadprog_solve_qp(P, q, G=None, h=None, A=None, b=None, initvals=None):
    # if initvals is not None:
    #     print("quadprog: note that warm-start values ignored by wrapper")
    qp_G = P
    qp_a = -q
    if A is not None:
        if G is None:
            qp_C = -A.T
            qp_b = -b
        else:
            qp_C = -vstack([A, G]).T
            qp_b = -hstack([b, h])
        meq = A.shape[0]
    else:  # no equality constraint
        qp_C = -G.T if G is not None else None
        qp_b = -h if h is not None else None
        meq = 0
    return solve_qp(qp_G, qp_a, qp_C, qp_b, meq)[0]