Exemple #1
0
    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
Exemple #4
0
    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
Exemple #5
0
    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)
Exemple #6
0
 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
Exemple #7
0
    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 = {}
Exemple #8
0
    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 = {}