def __init__(self, joints, kp=None, kd=None, gpos_des=None,
gvel_des=None, name=None):
Controller.__init__(self, name=name)
self._cndof = 0
dof_map = []
for j in joints:
if not isinstance(j, LinearConfigurationSpaceJoint):
raise ValueError('Joints must be ' +\
'LinearConfigurationSpaceJoint instances')
else:
self._cndof += j.ndof
dof_map.extend(range(j.dof.start, j.dof.stop))
self._dof_map = array(dof_map)
self.joints = joints
self._wndof = None
if kp is None:
self.kp = zeros((self._cndof, self._cndof))
else :
self.kp = array(kp).reshape((self._cndof, self._cndof))
if kd is None:
self.kd = zeros((self._cndof, self._cndof))
else :
self.kd = array(kd).reshape((self._cndof, self._cndof))
if gpos_des is None:
self.gpos_des = zeros(self._cndof)
else:
self.gpos_des = array(gpos_des).reshape(self._cndof)
if gvel_des is None:
self.gvel_des = zeros(self._cndof)
else:
self.gvel_des = array(gvel_des).reshape(self._cndof)
def __init__(self, world, tasks=[], name=None):
assert isinstance(world, World)
Controller.__init__(self, name=name)
self.world = world
self.frames = self.world.getframes()
self._rec_tau = [] # TODO: move this elsewhere
self._tasks = tasks
def __init__(self, world, tasks=[], name=None):
assert isinstance(world, World)
Controller.__init__(self, name=name)
self.world = world
self.frames = self.world.getframes()
self._rec_tau = [] #TODO: move this elsewhere
self._tasks = tasks
def __init__(self, gravity=-9.81, name=None):
""" Simulate gravity along the world up axis.
:param float gravity: the gravity magnitude along world up axis (can be negative)
:param name: the name of the controller
:type name: string or None
"""
Controller.__init__(self, name=name)
self.gravity = float(gravity)
self._bodies = None
self._wndof = None
self._gravity_dtwist = None
self._impedance = None
def __init__(self, joints, kp=None, kd=None, gpos_des=None, gvel_des=None, name=None):
""" Track an error with a proportional-derivative controller.
:param iterable joints: a list of :class:`~arboris.core.LinearConfigurationSpaceJoint` one wants to control (**x** is the total dimension of the controlled joints)
:param kp: the proportional gains
:type kp: (x,x)-array
:param kd: the derivative gains
:type kd: (x,x)-array
:param gpos_des: desired generalized position
:type gpos_des: (x,)-array
:param gvel_des: desired generalized velocity
:type gvel_des: (x,)-array
:param name: the name of the controller
"""
Controller.__init__(self, name=name)
self._cndof = 0
dof_map = []
for j in joints:
if not isinstance(j, LinearConfigurationSpaceJoint):
raise ValueError('Joints must be ' +\
'LinearConfigurationSpaceJoint instances')
else:
self._cndof += j.ndof
dof_map.extend(list(range(j.dof.start, j.dof.stop)))
self._dof_map = array(dof_map)
self.joints = joints
self._wndof = None
if kp is None:
self.kp = zeros((self._cndof, self._cndof))
else :
self.kp = array(kp).reshape((self._cndof, self._cndof))
if kd is None:
self.kd = zeros((self._cndof, self._cndof))
else :
self.kd = array(kd).reshape((self._cndof, self._cndof))
if gpos_des is None:
self.gpos_des = zeros(self._cndof)
else:
self.gpos_des = array(gpos_des).reshape(self._cndof)
if gvel_des is None:
self.gvel_des = zeros(self._cndof)
else:
self.gvel_des = array(gvel_des).reshape(self._cndof)
def __init__(self, gravity=-9.81, name=None):
self.gravity = float(gravity)
Controller.__init__(self, name=name)
self._bodies = None
self._wndof = None
self._gravity_dtwist = None
def __init__(self, gforcemax, dgforcemax=None, qlim=None, vlim=None, \
tasks=None, events=None, \
data=None, options=None, solver_options=None, name=None):
""" Initialize the instance of the LQP controller.
:param gforcemax: max gforce (N or Nm) {'joint_name': max_gforce}
:type gforcemax: dict
:param dgforcemax: max dgforce (N/s or Nm/s) {'joint_name': max_dgforce}
:type dgforcemax: dict
:param qlim: position limits (rad) {'joint_name': (qmin,qmax)}
:type qlim: dict
:param vlim: velocity limits (rad/s) {'joint_name': vmax}
:type vlim: dict
:param events: a list of events (from LQPctrl.events)
:type events: list
:param tasks: a list of tasks (from LQPctrl.tasks)
:type tasks: list
:param data: a dictionnary of data interesting to all component of LQP
:type data: dict
:param options: a dictionnary of options for the LQPctrl
:type options: dict
:param solver_options: a dictionnary of options for the solver
:type options: dict
:param name: the name of the LQP controller
"""
Controller.__init__(self, name=name)
self.gforcemax = dict(gforcemax)
self.dgforcemax = dgforcemax or {}
self.qlim = qlim or {}
self.vlim = vlim or {}
self.tasks = NamedObjectsList(tasks) if tasks is not None \
else NamedObjectsList([])
self.events = NamedObjectsList(events) if events is not None \
else NamedObjectsList([])
self.data = lqpc_data(data)
self.options = lqpc_options(options)
self.solver_options = lqpc_solver_options(solver_options)
self.world = None
self.bodies = []
self.WeightController = None
self._qlim = []
self._vlim = []
self.ndof = 0
self.S = zeros((0, 0))
self.constraints = []
self.is_enabled = {}
self._mus = []
self.n_fc = 0
self.Jc = zeros((0, 0))
self.dJc = zeros((0, 0))
self._gforcemax = []
self._dgforcemax = []
self._prec_gforce = None
self.n_chi = 0
self.n_gforce = 0
self.n_problem = 0
self.collision_shapes = []
self.X_solution = zeros(0)
self._gforce = zeros(0)
self.E_base = zeros((0, 0))
self.f_base = zeros(0)
self.cost = ""
self.norm = ""
self.formalism = ""
self._performance = {}
def __init__(self, gforcemax, dgforcemax=None, qlim=None, vlim=None, \
tasks=None, events=None, \
data=None, options=None, solver_options=None, name=None):
""" Initialize the instance of the LQP controller.
:param gforcemax: max gforce (N or Nm) {'joint_name': max_gforce}
:type gforcemax: dict
:param dgforcemax: max dgforce (N/s or Nm/s) {'joint_name': max_dgforce}
:type dgforcemax: dict
:param qlim: position limits (rad) {'joint_name': (qmin,qmax)}
:type qlim: dict
:param vlim: velocity limits (rad/s) {'joint_name': vmax}
:type vlim: dict
:param events: a list of events (from LQPctrl.events)
:type events: list
:param tasks: a list of tasks (from LQPctrl.tasks)
:type tasks: list
:param data: a dictionnary of data interesting to all component of LQP
:type data: dict
:param options: a dictionnary of options for the LQPctrl
:type options: dict
:param solver_options: a dictionnary of options for the solver
:type options: dict
:param name: the name of the LQP controller
"""
Controller.__init__(self, name=name)
self.gforcemax = dict(gforcemax)
self.dgforcemax = dgforcemax or {}
self.qlim = qlim or {}
self.vlim = vlim or {}
self.tasks = NamedObjectsList(tasks) if tasks is not None \
else NamedObjectsList([])
self.events = NamedObjectsList(events) if events is not None \
else NamedObjectsList([])
self.data = lqpc_data(data)
self.options = lqpc_options(options)
self.solver_options = lqpc_solver_options(solver_options)
self.world = None
self.bodies = []
self.WeightController = None
self._qlim = []
self._vlim = []
self.ndof = 0
self.S = zeros((0, 0))
self.constraints = []
self.is_enabled = {}
self._mus = []
self.n_fc = 0
self.Jc = zeros((0, 0))
self.dJc = zeros((0, 0))
self._gforcemax = []
self._dgforcemax = []
self._prec_gforce = None
self.n_chi = 0
self.n_gforce = 0
self.n_problem = 0
self.collision_shapes = []
self.X_solution = zeros(0)
self._gforce = zeros(0)
self.E_base = zeros((0, 0))
self.f_base = zeros(0)
self.cost = ""
self.norm = ""
self.formalism = ""
self._performance = {}
