From 309c4712afd3fffb2ae6364eaf69c100f18115b1 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Mon, 22 Jul 2019 02:27:42 +0200 Subject: [PATCH] update actuators --- pyrobolearn/robots/actuators/__init__.py | 3 +- pyrobolearn/robots/actuators/actuator.py | 66 ++- pyrobolearn/robots/actuators/joints.py | 491 ++++++++++++++++------- pyrobolearn/robots/sensors/joints.py | 7 +- pyrobolearn/robots/sensors/sensor.py | 2 +- 5 files changed, 406 insertions(+), 163 deletions(-) diff --git a/pyrobolearn/robots/actuators/__init__.py b/pyrobolearn/robots/actuators/__init__.py index 12e36f3..f3f7e75 100644 --- a/pyrobolearn/robots/actuators/__init__.py +++ b/pyrobolearn/robots/actuators/__init__.py @@ -3,7 +3,8 @@ from .actuator import Actuator # import joint actuators -from .joints import * +from .joints import JointActuator, JointPositionActuator, JointVelocityActuator, JointPositionVelocityActuator, \ + JointTorqueActuator # import speaker from .speaker import Speaker diff --git a/pyrobolearn/robots/actuators/actuator.py b/pyrobolearn/robots/actuators/actuator.py index 5522eb0..3474b5f 100644 --- a/pyrobolearn/robots/actuators/actuator.py +++ b/pyrobolearn/robots/actuators/actuator.py @@ -7,7 +7,9 @@ other joint actuators. Additionally, this is important as more realistic motors simulation to reality. """ -from pyrobolearn.robots.noise.noise import Noise, NoNoise +from abc import ABCMeta + +from pyrobolearn.utils.data_structures.queues import FIFOQueue __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -25,20 +27,27 @@ class Actuator(object): All actuator classes inherit from this class. Actuators such as motors are often attached to the robot joints. Other actuators such as speakers, leds, and others are attached to links. """ + __metaclass__ = ABCMeta - def __init__(self, noise=None, latency=0): + def __init__(self, ticks=1, latency=0): """ Initialize the actuator. Args: - noise (None, Noise): noise to be added. + ticks (int): number of steps to wait/sleep before acting in the world. latency (int, float, None): latency time / step. """ # variable to check if the actuator is enabled self._enabled = True + # set the ticks + self._ticks = ticks + self._cnt = -1 + # set the latency + if latency is None: + latency = 0 if not isinstance(latency, (int, float)): raise TypeError("Expecting the given 'latency' to be an int or float, instead got: " "{}".format(type(latency))) @@ -46,17 +55,10 @@ class Actuator(object): raise ValueError("Expecting the given 'latency' to be a positive number, but got instead: " "{}".format(latency)) self._latency = latency - self._latent_cnt = -1 + self._data_queue = FIFOQueue(maxsize=self._latency + 1) # latency is modeled using a queue - # set the noise - if noise is None: - noise = NoNoise() - if not isinstance(noise, Noise): - raise TypeError("Expecting the given 'noise' to be an instance of Noise, instead got: " - "{}".format(type(noise))) - self._noise = noise - - # self.sim = simulator + # self.sim = simulator + self._data = None ############## # Properties # @@ -72,35 +74,61 @@ class Actuator(object): @property def enabled(self): - """Return if the sensor is enabled or not.""" + """Return if the actuator is enabled or not.""" return self._enabled @property def disabled(self): - """Return if the sensor is disabled or not.""" + """Return if the actuator is disabled or not.""" return not self._enabled + @property + def data(self): + """Return the data.""" + return self._data + + @data.setter + def data(self, data): + """Set the data.""" + while len(self._data_queue) != self._data_queue.maxsize: # fill the data queue + self._data_queue.append(data) + ########### # Methods # ########### def enable(self): - """Enable the sensor.""" + """Enable the actuator.""" self._enabled = True def disable(self): - """Disable the sensor.""" + """Disable the actuator.""" self._enabled = False - def compute(self, *args, **kwargs): # TODO: call it actuate? + def compute(self, *args, **kwargs): pass + def act(self): + """Set the next actuator value.""" + if self._enabled: + self._cnt += 1 + if (self._cnt % self._ticks) == 0: + self._data = self._data_queue.get() + self._act() + self._cnt = 0 + + def _act(self): + """Act method to be implemented in the child class.""" + raise NotImplementedError + ############# # Operators # ############# def __call__(self, *args, **kwargs): - return self.compute(*args, **kwargs) + """Set the next actuator value.""" + self.act() + # return self.compute(*args, **kwargs) # def __repr__(self): # """Return a representation string about the class for debugging and development.""" diff --git a/pyrobolearn/robots/actuators/joints.py b/pyrobolearn/robots/actuators/joints.py index 0d87ec9..6bb63ba 100644 --- a/pyrobolearn/robots/actuators/joints.py +++ b/pyrobolearn/robots/actuators/joints.py @@ -6,7 +6,9 @@ import copy import numpy as np from abc import ABCMeta +from pyrobolearn.simulators.simulator import Simulator from pyrobolearn.robots.actuators.actuator import Actuator +from pyrobolearn.robots.base import Body __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -28,20 +30,73 @@ class JointActuator(Actuator): """ __metaclass__ = ABCMeta - def __init__(self, joint_id, latency=None): + def __init__(self, simulator, body_id, joint_ids, ticks=1, latency=None): """ Initialize the joint actuator. Args: - joint_id (int): joint unique id. + simulator (Simulator): simulator instance. + body_id (int): unique body id. + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints. + ticks (int): number of steps to wait/sleep before acting in the world. latency (int, float, None): latency time / step. """ - super(JointActuator, self).__init__(latency=latency) - self.joint_id = joint_id + super(JointActuator, self).__init__(ticks=ticks, latency=latency) + + # setting simulator + if not isinstance(simulator, Simulator): + raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: " + "{}".format(type(simulator))) + self.sim = simulator + + # set the body id + if isinstance(body_id, Body): + body_id = body_id.id + elif not isinstance(body_id, (int, long)): + raise TypeError("Expecting the given 'body_id' to be an int or an instance of `Body`, but got instead: " + "{}".format(type(body_id))) + if body_id < 0: + raise ValueError("Expecting the given 'body_id' to be a positive integer, but got instead: " + "{}".format(body_id)) + self.body_id = body_id + + # set the joint ids + if joint_ids is None: + # get actuated joints + joint_ids = [] + for joint_id in range(self.sim.num_joints(self.body_id)): + joint_info = self.sim.get_joint_info(self.body_id, joint_id) + if joint_info[2] != self.sim.JOINT_FIXED: # if not a fixed joint + joint_ids.append(joint_info[0]) + elif isinstance(joint_ids, int): + joint_ids = [joint_ids] + elif isinstance(joint_ids, (tuple, list)): + for i, joint in enumerate(joint_ids): + if not isinstance(joint, int): + raise TypeError("Expecting the given {}th joint id to be an int, instead got: {}".format(i, joint)) + else: + raise TypeError("Expecting the given 'joint_ids' to be an int or list of int, instead got: " + "{}".format(joint_ids)) + self.joint_ids = joint_ids + self.q_indices = self.sim.get_q_indices(self.body_id, self.joint_ids) + + ############## + # Properties # + ############## + + @property + def simulator(self): + """Return the simulator instance.""" + return self.sim + + ############# + # Operators # + ############# def __copy__(self): """Return a shallow copy of the actuator. This can be overridden in the child class.""" - return self.__class__(joint_id=self.joint_id) + return self.__class__(simulator=self.sim, body_id=self.body_id, joint_ids=self.joint_ids, ticks=self._ticks, + latency=self._latency) def __deepcopy__(self, memo={}): """Return a deep copy of the actuator. This can be overridden in the child class. @@ -49,167 +104,325 @@ class JointActuator(Actuator): Args: memo (dict): memo dictionary of objects already copied during the current copying pass """ - joint_id = copy.deepcopy(self.joint_id) - actuator = self.__class__(joint_id=joint_id) + if self in memo: + return memo[self] + simulator = memo.get(self.simulator, self.simulator) # copy.deepcopy(self.simulator, memo) + joint_ids = copy.deepcopy(self.joint_ids) + actuator = self.__class__(simulator=simulator, body_id=self.body_id, joint_ids=joint_ids, ticks=self._ticks, + latency=self._latency) memo[self] = actuator return actuator -class PDJointActuator(JointActuator): - r"""PD Joint Actuator +class JointPositionActuator(JointActuator): + r"""Joint position actuator - Compute the torque to be applied on the joint using a PD controller: :math:`\tau = k_p (q_d - q) - k_d \dot{q}`, - where :math:`q` and :math:`\dot{q}` are the current joint position and velocity respectively, :math:`q_d` is - the desired joint position, and :math:`k_p` and :math:`k_d` are the PD gains. """ - def __init__(self, joint_id, kp=0, kd=0, min_torque=-np.infty, max_torque=np.infty, latency=0): + def __init__(self, simulator, body_id, joint_ids, kps=None, kds=None, forces=None, ticks=1, latency=None): """ - Initialize the PD joint actuator. + Initialize the joint position actuator. Args: - joint_id (int): joint id. - kp (float): position gain - kd (float): velocity gain - min_torque (float): minimum torque - max_torque (float): maximum torque + simulator (Simulator): simulator instance. + body_id (int): unique body id. + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints. + kps (None, float, np.array[N]): position gain(s) + kds (None, float, np.array[N]): velocity gain(s) + forces (None, float, np.array[N]): maximum motor force(s)/torque(s) used to reach the target values. + ticks (int): number of steps to wait/sleep before acting in the world. latency (int, float, None): latency time / step. """ - super(PDJointActuator, self).__init__(joint_id, latency=latency) - self.kp = kp - self.kd = kd - self.min_torque = min_torque - self.max_torque = max_torque + super(JointPositionActuator, self).__init__(simulator=simulator, body_id=body_id, joint_ids=joint_ids, + ticks=ticks, latency=latency) + # set gains + self._kps = kps + self._kds = kds + self._forces = forces - def compute(self, qd, q, dq): - """ - Compute and return the torque using the PD control scheme. + def set_joint_positions(self, positions, velocities=None): + r""" + Set the position of the given joint(s) (using position control). Args: - qd (float): desired joint position - q (float): current joint position - dq (float): current joint velocity - - Returns: - float: computed torque using PD control + positions (float, np.array[N]): desired position, or list of desired positions [rad] + velocities (float, np.array[N], None): desired velocity, or list of desired velocities [rad/s] """ - torque = self.kp * (qd - q) - self.kd * dq - torque = np.clip(torque, self.min_torque, self.max_torque) - return torque + self.sim.set_joint_positions(body_id=self.body_id, joint_ids=self.joint_ids, positions=positions, + velocities=velocities, kps=self._kps, kds=self._kds, forces=self._forces) - def __copy__(self): - """Return a shallow copy of the actuator. This can be overridden in the child class.""" - return self.__class__(joint_id=self.joint_id, kp=self.kp, kd=self.kd, min_torque=self.min_torque, - max_torque=self.max_torque, latency=self.latency) + def _act(self): + """Act using the actuator by setting the joint positions.""" + self.set_joint_positions(positions=self._data) - def __deepcopy__(self, memo={}): - """Return a deep copy of the actuator. This can be overridden in the child class. + +class JointVelocityActuator(JointActuator): + r"""Joint velocity actuator + + """ + + def __init__(self, simulator, body_id, joint_ids, max_force=None, ticks=1, latency=None): + """ + Initialize the joint velocity actuator. Args: - memo (dict): memo dictionary of objects already copied during the current copying pass + simulator (Simulator): simulator instance. + body_id (int): unique body id. + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints. + max_force (float, float[N], None): maximum allowed force/torque for each joint. + ticks (int): number of steps to wait/sleep before acting in the world. + latency (int, float, None): latency time / step. """ - joint_id = copy.deepcopy(self.joint_id) - kp = copy.deepcopy(self.kp) - kd = copy.deepcopy(self.kd) - min_torque = copy.deepcopy(self.min_torque) - max_torque = copy.deepcopy(self.max_torque) - latency = copy.deepcopy(self.latency) - actuator = self.__class__(joint_id=joint_id, kp=kp, kd=kd, min_torque=min_torque, max_torque=max_torque, - latency=latency) - memo[self] = actuator - return actuator + super(JointVelocityActuator, self).__init__(simulator=simulator, body_id=body_id, joint_ids=joint_ids, + ticks=ticks, latency=latency) + # set max force + self._max_force = max_force -class GearedActuator(JointActuator): - r"""Geared Actuator - """ - - def __init__(self, joint_id): - super(GearedActuator, self).__init__(joint_id) - - -class DirectDriveActuator(JointActuator): - r"""Direct Drive Actuator - """ - - def __init__(self, joint_id): - super(DirectDriveActuator, self).__init__(joint_id) - - -class SEA(JointActuator): - r"""Series Elastic Actuators - - This actuator has multiple components including springs, gears, encoders, and an electric motors, resulting in - complex dynamics. Specifically, it is composed of an electric motor, a high gear ratio transmission, an elastic - element, and two rotary encoders to measure spring deflection and output position. [2] - - References: - [1] "Series elastic actuators", Pratt et al., 1995 - [2] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019 - """ - - def __init__(self, joint_id): - super(SEA, self).__init__(joint_id) - - -class HydraulicActuator(JointActuator): - r"""Hydraulic Actuator - """ - - def __init__(self, joint_id): - super(HydraulicActuator, self).__init__(joint_id) - - -class JointActuatorApproximator(JointActuator): - r"""Joint Actuator Approximator. - - This is a joint actuator that uses a function approximator to output the torque values to be applied on the - actuator given for instance the joint positions. This function approximator has been trained on real data obtained - from the real actuator and can thus be a better approximation of the way the actual actuator works. - """ - - def __init__(self, joint_id, approximator=None): - super(JointActuatorApproximator, self).__init__(joint_id) - self.approximator = approximator - - def __copy__(self): - """Return a shallow copy of the actuator. This can be overridden in the child class.""" - return self.__class__(joint_id=self.joint_id, approximator=self.approximator) - - def __deepcopy__(self, memo={}): - """Return a deep copy of the actuator. This can be overridden in the child class. + def set_joint_velocities(self, velocities): + r""" + Set the velocity of the given joint(s) (using velocity control). Args: - memo (dict): memo dictionary of objects already copied during the current copying pass + velocities (float, np.array[N]): desired velocity, or list of desired velocities [rad/s] """ - joint_id = copy.deepcopy(self.joint_id) - approximator = copy.deepcopy(self.approximator, memo) - actuator = self.__class__(joint_id=joint_id, approximator=approximator) - memo[self] = actuator - return actuator + self.sim.set_joint_velocities(body_id=self.body_id, joint_ids=self.joint_ids, velocities=velocities, + max_force=self._max_force) + + def _act(self): + """Act using the actuator by setting the joint velocities.""" + self.set_joint_velocities(velocities=self._data) -class ActuatorNet(JointActuatorApproximator): - r"""Actuator Neural Network - - References: - [1] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019 - """ - - def __init__(self, joint_id, nn_model=None): - super(ActuatorNet, self).__init__(joint_id, approximator=nn_model) - - -class CoupledJointActuatorApproximator(JointActuator): - r"""Coupled Joint Actuator Approximator - - Multiple joint ids. - """ - pass - - -class CoupledActuatorNet(CoupledJointActuatorApproximator): - r"""Coupled Actuator Neural Network +class JointPositionVelocityActuator(JointPositionActuator): + r"""Joint position velocity actuator """ - pass + + def __init__(self, simulator, body_id, joint_ids, kps=None, kds=None, forces=None, ticks=1, latency=None): + """ + Initialize the joint position velocity actuator. + + Args: + simulator (Simulator): simulator instance. + body_id (int): unique body id. + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints. + kps (None, float, np.array[N]): position gain(s) + kds (None, float, np.array[N]): velocity gain(s) + forces (None, float, np.array[N]): maximum motor force(s)/torque(s) used to reach the target values. + ticks (int): number of steps to wait/sleep before acting in the world. + latency (int, float, None): latency time / step. + """ + super(JointPositionVelocityActuator, self).__init__(simulator=simulator, body_id=body_id, joint_ids=joint_ids, + ticks=ticks, latency=latency) + # set gains + self._kps = kps + self._kds = kds + self._forces = forces + + def _act(self): + """Act using the actuator by setting the joint positions and velocities.""" + middle_idx = int(len(self._data) / 2) + positions, velocities = self._data[:middle_idx], self._data[middle_idx:] + self.set_joint_positions(positions=positions, velocities=velocities) + + +class JointTorqueActuator(JointActuator): + r"""Joint torque actuator + + """ + + def __init__(self, simulator, body_id, joint_ids, ticks=1, latency=None): + """ + Initialize the joint torque actuator. + + Args: + simulator (Simulator): simulator instance. + body_id (int): unique body id. + joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints. + ticks (int): number of steps to wait/sleep before acting in the world. + latency (int, float, None): latency time / step. + """ + super(JointTorqueActuator, self).__init__(simulator=simulator, body_id=body_id, joint_ids=joint_ids, + ticks=ticks, latency=latency) + + def set_joint_torques(self, torques=None): + r""" + Set the torque to the given joint(s) (using force/torque control). + + Args: + torques (float, np.array[N], None): desired torque(s) to apply to the joint(s) [N]. If None, it will apply + a torque of 0 to the given joint(s). + """ + if torques is None: + torques = [0] * len(self.joint_ids) + elif isinstance(torques, (int, float)): + torques = [torques] * len(self.joint_ids) + + self.sim.set_joint_torques(self.body_id, joint_ids=self.joint_ids, torques=torques) + + def _act(self): + """Act using the actuator by setting the joint torques.""" + self.set_joint_torques(torques=self._data) + + +############################################################ + +# class PDJointActuator(JointActuator): # see also utils/feedback.py +# r"""PD Joint Actuator +# +# Compute the torque to be applied on the joint using a PD controller: :math:`\tau = k_p (q_d - q) - k_d \dot{q}`, +# where :math:`q` and :math:`\dot{q}` are the current joint position and velocity respectively, :math:`q_d` is +# the desired joint position, and :math:`k_p` and :math:`k_d` are the PD gains. +# """ +# +# def __init__(self, joint_id, kp=0, kd=0, min_torque=-np.infty, max_torque=np.infty, latency=0): +# """ +# Initialize the PD joint actuator. +# +# Args: +# joint_id (int): joint id. +# kp (float): position gain +# kd (float): velocity gain +# min_torque (float): minimum torque +# max_torque (float): maximum torque +# latency (int, float, None): latency time / step. +# """ +# super(PDJointActuator, self).__init__(joint_id, latency=latency) +# self.kp = kp +# self.kd = kd +# self.min_torque = min_torque +# self.max_torque = max_torque +# +# def compute(self, qd, q, dq): +# """ +# Compute and return the torque using the PD control scheme. +# +# Args: +# qd (float): desired joint position +# q (float): current joint position +# dq (float): current joint velocity +# +# Returns: +# float: computed torque using PD control +# """ +# torque = self.kp * (qd - q) - self.kd * dq +# torque = np.clip(torque, self.min_torque, self.max_torque) +# return torque +# +# def __copy__(self): +# """Return a shallow copy of the actuator. This can be overridden in the child class.""" +# return self.__class__(joint_id=self.joint_id, kp=self.kp, kd=self.kd, min_torque=self.min_torque, +# max_torque=self.max_torque, latency=self.latency) +# +# def __deepcopy__(self, memo={}): +# """Return a deep copy of the actuator. This can be overridden in the child class. +# +# Args: +# memo (dict): memo dictionary of objects already copied during the current copying pass +# """ +# joint_id = copy.deepcopy(self.joint_id) +# kp = copy.deepcopy(self.kp) +# kd = copy.deepcopy(self.kd) +# min_torque = copy.deepcopy(self.min_torque) +# max_torque = copy.deepcopy(self.max_torque) +# latency = copy.deepcopy(self.latency) +# actuator = self.__class__(joint_id=joint_id, kp=kp, kd=kd, min_torque=min_torque, max_torque=max_torque, +# latency=latency) +# memo[self] = actuator +# return actuator +# +# +# class GearedActuator(JointActuator): +# r"""Geared Actuator +# """ +# +# def __init__(self, joint_id): +# super(GearedActuator, self).__init__(joint_id) +# +# +# class DirectDriveActuator(JointActuator): +# r"""Direct Drive Actuator +# """ +# +# def __init__(self, joint_id): +# super(DirectDriveActuator, self).__init__(joint_id) +# +# +# class SEA(JointActuator): +# r"""Series Elastic Actuators +# +# This actuator has multiple components including springs, gears, encoders, and an electric motors, resulting in +# complex dynamics. Specifically, it is composed of an electric motor, a high gear ratio transmission, an elastic +# element, and two rotary encoders to measure spring deflection and output position. [2] +# +# References: +# [1] "Series elastic actuators", Pratt et al., 1995 +# [2] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019 +# """ +# +# def __init__(self, joint_id): +# super(SEA, self).__init__(joint_id) +# +# +# class HydraulicActuator(JointActuator): +# r"""Hydraulic Actuator +# """ +# +# def __init__(self, joint_id): +# super(HydraulicActuator, self).__init__(joint_id) +# +# +# class JointActuatorApproximator(JointActuator): +# r"""Joint Actuator Approximator. +# +# This is a joint actuator that uses a function approximator to output the torque values to be applied on the +# actuator given for instance the joint positions. This function approximator has been trained on real data obtained +# from the real actuator and can thus be a better approximation of the way the actual actuator works. +# """ +# +# def __init__(self, joint_id, approximator=None): +# super(JointActuatorApproximator, self).__init__(joint_id) +# self.approximator = approximator +# +# def __copy__(self): +# """Return a shallow copy of the actuator. This can be overridden in the child class.""" +# return self.__class__(joint_id=self.joint_id, approximator=self.approximator) +# +# def __deepcopy__(self, memo={}): +# """Return a deep copy of the actuator. This can be overridden in the child class. +# +# Args: +# memo (dict): memo dictionary of objects already copied during the current copying pass +# """ +# joint_id = copy.deepcopy(self.joint_id) +# approximator = copy.deepcopy(self.approximator, memo) +# actuator = self.__class__(joint_id=joint_id, approximator=approximator) +# memo[self] = actuator +# return actuator +# +# +# class ActuatorNet(JointActuatorApproximator): +# r"""Actuator Neural Network +# +# References: +# [1] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019 +# """ +# +# def __init__(self, joint_id, nn_model=None): +# super(ActuatorNet, self).__init__(joint_id, approximator=nn_model) +# +# +# class CoupledJointActuatorApproximator(JointActuator): +# r"""Coupled Joint Actuator Approximator +# +# Multiple joint ids. +# """ +# pass +# +# +# class CoupledActuatorNet(CoupledJointActuatorApproximator): +# r"""Coupled Actuator Neural Network +# +# """ +# pass diff --git a/pyrobolearn/robots/sensors/joints.py b/pyrobolearn/robots/sensors/joints.py index d154faf..fd071e7 100644 --- a/pyrobolearn/robots/sensors/joints.py +++ b/pyrobolearn/robots/sensors/joints.py @@ -29,10 +29,11 @@ class JointSensor(Sensor): __metaclass__ = ABCMeta def __init__(self, simulator, body_id, joint_ids=None, noise=None, ticks=1, latency=None): - """Initialize the sensor. + """ + Initialize the joint sensor. Args: - simulator (Simulator): simulator + simulator (Simulator): simulator instance. body_id (int): unique body id. joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints. noise (None, Noise): noise to be added. @@ -222,7 +223,7 @@ class JointEncoderSensor(JointSensor): Initialize the joint encoder sensor. Args: - simulator (Simulator): simulator + simulator (Simulator): simulator instance. body_id (int): unique body id. joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will get all the actuated joints. noise (None, Noise): noise to be added. diff --git a/pyrobolearn/robots/sensors/sensor.py b/pyrobolearn/robots/sensors/sensor.py index f7d3925..d9f97c3 100644 --- a/pyrobolearn/robots/sensors/sensor.py +++ b/pyrobolearn/robots/sensors/sensor.py @@ -114,7 +114,7 @@ class Sensor(object): # sensor attached to a link or joint if latency < 0: raise ValueError("Expecting the given 'latency' to be a positive number, but got instead: " "{}".format(latency)) - self._latency = latency + self._latency = latency + 1 self._latent_cnt = -1 # data from last acquisition