-
Notifications
You must be signed in to change notification settings - Fork 62
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Some problem in the pyrobolearn/kinematics/inverse/moving_sphere.py #26
Comments
the red line is the desired trajectory and the blue line is the actual end-effector pose |
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
You could first compare if your function error_quaternion returns the same thing as that function.
Also, there is another example in the folder pyrobolearn/pyrobolearn/priorities/ik.py Line 38 in b1f4ac8
I will soon clean that file Best, |
@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: |
Hi, I just launched the
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 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] |
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])
The text was updated successfully, but these errors were encountered: