def gather_config(self, config_all, vector_sub=None):
"""Copy elements of a full config into the sub config vector."""
# TODO: convert dictionary
if vector_sub is None:
vector_sub = DVec(self.config_count)
libamino.aa_rx_sg_sub_config_gather(self._ptr, config_all, vector_sub)
return vector_sub
def solve_vel(self, dq=None):
"""Solve for velocity.
Returns:
sub-scenegraph configuration velocity or None if no solution.
"""
if (self._tf_ref is None) and (self._tf_vel_ref is None):
raise Exception("No reference provided.")
if dq is None:
dq = DVec(self.ssg.config_count)
dx = self.dx
# velocity reference
if self._tf_vel_ref:
dx.copy_from(self._tf_vel_ref)
else:
dx.zero()
# position reference
if self._tf_ref:
tf_act = self.fk[self.ssg.end_effector_id]
libamino.aa_rx_wk_dx_pos(self.opts, tf_act, self._tf_ref, dx)
# configuration reference
# TODO
r = libamino.aa_rx_wk_dx2dq(self.ssg._ptr, self.opts, self.fk._ptr, dx,
dq)
return dq if r == 0 else None
def __init__(self, sub_scenegraph, scene_fk):
self.ssg = sub_scenegraph
self.fk = scene_fk
self.opts = libamino.aa_rx_wk_opts_create()
self._tf_ref = None
self._tf_vel_ref = None
self._config_ref = None
self.dx = DVec(6)
def config_vector(self, config, vector=None):
"""Convert config to a vector."""
# TODO: convert dictionary
if vector is None:
return DVec.ensure(config)
else:
vector.copy_from(config)
return vector
def scatter_config(self, config_sub, vector_all=None):
"""Copy elements of a sub-scenegraph config into the full config vector."""
if isinstance(config_sub, dict):
return self.scenegraph.config_vector(config_sub, vector_all)
if vector_all is None:
vector_all = DVec(self.scenegraph.config_count)
libamino.aa_rx_sg_sub_config_scatter(self._ptr, config_sub, vector_all)
return vector_all
def solve(self):
"""
Solves the IK problem.
Returns:
The sub-scenegraph configuration vector or None if no solution.
"""
if self._tf_ref is None:
raise Exception("No reference provided.")
M = DMat((len(self._tf_ref), 1))
M.col_vec(0).copy_from(self._tf_ref)
q = DVec(self.ssg.config_count)
r = libamino.aa_rx_ik_solve(self._ptr, M, q)
return q if r == 0 else None
def config_vector(self, config, vector=None):
"""Create or convert to a configuration vector.
Raises:
IndexError: provided vector is the wrong size"""
if isinstance(config, dict):
if vector is None:
vector = DVec(self.config_count)
vector.set(0)
elif len(vector) != self.config_count:
raise IndexError()
for key in config:
vector[self.config_id(key)] = config[key]
return vector
elif vector is None:
return DVec.ensure(config)
else:
vector.copy_from(config)
return vector
def config(self):
"""The current configuration."""
return DVec(self._q)
def scenegraph(self, scenegraph):
self._scenegraph = scenegraph
self._q = DVec(scenegraph.config_count)
LIBAMINOGL.aa_rx_win_set_sg(self._ptr, scenegraph._ptr)
def to_dvec(self, vec=DVec(6)):
"""Copies self to a vec."""
vec.copy_from(self)
return vec
def __init__(self, scenegraph):
self.scenegraph = scenegraph
self._ptr = libamino.aa_rx_fk_malloc(scenegraph._ptr)
self._q = DVec(scenegraph.config_count)
def copy_config(self, config):
"""Copy configuration as a vector."""
return self.config_vector(config, DVec(self.config_count))