I guess what you want is to also control the orientation of the end effector. Unfortunately this has not yet been implemented in the ikpy library we are using. Feel free to push @phylliade to do it
I look at your ik.py and the ikpy lib.
I am new at Python, but if I understand well, inverse_kinematic of the lib give you a matrice of posible combination of angles, you convert it and for each motors and angles you call goto_position.
But I don’t understand where and how you choose the combination youy will use.
I think it is in convert_from_ik_angles.
def convert_from_ik_angles(self, joints):
""" Convert from IKPY internal representation to poppy representation. """
if len(joints) != len(self.motors) + 2:
raise ValueError('Incompatible data, len(joints) should be {}!'.format(len(self.motors) + 2))
joints = [rad2deg(j) for j in joints[1:-1]]
joints *= self._reversed
return [(j * (1 if m.direct else -1)) - m.offset
for j, m in zip(joints, self.motors)]
Actually IKpy gives you a single “optimised” solution, so on the pypot side we don’t have to choose. Note that in IKPy you can choose what optimisation technique will be used depending on what you want to do (e.g. a fast vs accurate method).
Unfortunately I’m not an expert in how IKpy works, maybe @phylliade (He’s the one behind this library) can give you more details.
I start looking at the IKpy lib.
It is the optimization method that choose angles to return.
In the inverse_kinematic.py of ikpy I try to make some changes to add constraints:
I change the optimization algo (from L-BFGS-B to SLSQP)
I add constraints
It work well for now
# coding= utf8
import scipy.optimize
import numpy as np
from . import logs
def inverse_kinematic_optimization(chain, target_frame, starting_nodes_angles, regularization_parameter=None, max_iter=None):
"""Computes the inverse kinematic on the specified target with an optimization method
:param ikpy.chain.Chain chain: The chain used for the Inverse kinematics.
:param numpy.array target: The desired target.
:param numpy.array starting_nodes_angles: The initial pose of your chain.
:param float regularization_parameter: The coefficient of the regularization.
:param int max_iter: Maximum number of iterations for the optimisation algorithm.
"""
# Only get the position
target = target_frame[:3, 3]
if starting_nodes_angles is None:
raise ValueError("starting_nodes_angles must be specified")
# Compute squared distance to target
def optimize_target(x):
# y = np.append(starting_nodes_angles[:chain.first_active_joint], x)
y = chain.active_to_full(x, starting_nodes_angles)
squared_distance = np.linalg.norm(chain.forward_kinematics(y)[:3, -1] - target)
return squared_distance
# If a regularization is selected
if regularization_parameter is not None:
def optimize_total(x):
regularization = np.linalg.norm(x - starting_nodes_angles[chain.first_active_joint:])
return optimize_target(x) + regularization_parameter * regularization
else:
def optimize_total(x):
return optimize_target(x)
# Add Constraints
cons = ({'type':'eq','fun': lambda x: x[1] + x[2] + x[4] + x[5]},
{'type':'eq','fun': lambda x: x[3]})
# Compute bounds
real_bounds = [link.bounds for link in chain.links]
# real_bounds = real_bounds[chain.first_active_joint:]
real_bounds = chain.active_from_full(real_bounds)
options = {}
# Manage iterations maximum
if max_iter is not None:
options["maxiter"] = max_iter
# Utilisation d'une optimisation SLSQP_**
res = scipy.optimize.minimize(optimize_total, chain.active_from_full(starting_nodes_angles), method='SLSQP', bounds=real_bounds, options=options, constraints=cons)
# Utilisation d'une optimisation L-BFGS-B
# res = scipy.optimize.minimize(optimize_total, chain.active_from_full(starting_nodes_angles), method='L-BFGS-B', bounds=real_bounds, options=options)
logs.manager.info("Inverse kinematic optimisation OK, done in {} iterations".format(res.nit))
return(chain.active_to_full(res.x, starting_nodes_angles))
# return(np.append(starting_nodes_angles[:chain.first_active_joint], res.x))
