diff --git a/pyrobolearn/actions/action.py b/pyrobolearn/actions/action.py index 635e5e1..1e9b171 100644 --- a/pyrobolearn/actions/action.py +++ b/pyrobolearn/actions/action.py @@ -441,7 +441,7 @@ class Action(object): append = add extend = add - def _write(self, data=None): + def _write(self, data): pass def write(self, data=None): @@ -453,10 +453,16 @@ class Action(object): if self.cnt % self.ticks == 0: if self.has_data(): # write the current action + if data is None: + data = self._data self._write(data) else: # read each action if self.actions: + if data is None: + data = [None] * len(self.actions) for action, d in zip(self.actions, data): + if d is None: + d = action._data action._write(d) self.cnt += 1 diff --git a/pyrobolearn/actions/basic_actions.py b/pyrobolearn/actions/basic_actions.py index 079349c..a98f924 100644 --- a/pyrobolearn/actions/basic_actions.py +++ b/pyrobolearn/actions/basic_actions.py @@ -28,9 +28,6 @@ class FixedAction(Action): def __init__(self, value): super(FixedAction, self).__init__(data=value) - def _write(self, data=None): - pass - def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" return self.__class__(value=self._data) @@ -59,7 +56,7 @@ class FunctionalAction(Action): self.function = function super(FunctionalAction, self).__init__(data=initial_data) - def _write(self, data=None): + def _write(self, data): self.data = self.function(data) def __copy__(self): diff --git a/pyrobolearn/actions/gym_actions.py b/pyrobolearn/actions/gym_actions.py index b2c082e..3fddd1a 100644 --- a/pyrobolearn/actions/gym_actions.py +++ b/pyrobolearn/actions/gym_actions.py @@ -46,9 +46,6 @@ class GymAction(Action): # call super constructor super(GymAction, self).__init__(data=data, space=space) - def _write(self, data=None): - pass - def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" return self.__class__(gym_env=self.env) diff --git a/pyrobolearn/actions/robot_actions/joint_actions.py b/pyrobolearn/actions/robot_actions/joint_actions.py index cf4ca26..b9d7604 100644 --- a/pyrobolearn/actions/robot_actions/joint_actions.py +++ b/pyrobolearn/actions/robot_actions/joint_actions.py @@ -48,6 +48,7 @@ class JointAction(RobotAction): # return len(self.joints) def bounds(self): + """Return the joint limits.""" return self.robot.get_joint_limits(self.joints) def __copy__(self): @@ -80,11 +81,9 @@ class JointPositionAction(JointAction): super(JointPositionAction, self).__init__(robot, joint_ids) self.data = robot.get_joint_positions(self.joints) - def _write(self, data=None): - if data is None: - self.robot.set_joint_positions(self._data, self.joints, kp=self.kp, kd=self.kd, forces=self.max_force) - else: - self.robot.set_joint_positions(data, self.joints, kp=self.kp, kd=self.kd, forces=self.max_force) + def _write(self, data): + """apply the action data on the robot.""" + self.robot.set_joint_positions(data, self.joints, kp=self.kp, kd=self.kd, forces=self.max_force) def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" @@ -118,11 +117,9 @@ class JointVelocityAction(JointAction): super(JointVelocityAction, self).__init__(robot, joint_ids) self.data = robot.get_joint_velocities(self.joints) - def _write(self, data=None): - if data is None: - self.robot.set_joint_velocities(self._data, self.joints) - else: - self.robot.set_joint_velocities(data, self.joints) + def _write(self, data): + """apply the action data on the robot.""" + self.robot.set_joint_velocities(data, self.joints) class JointPositionAndVelocityAction(JointAction): @@ -139,13 +136,10 @@ class JointPositionAndVelocityAction(JointAction): self.data = np.concatenate((pos, vel)) self.idx = len(pos) - def _write(self, data=None): - if data is None: - self.robot.set_joint_positions(self._data[:self.idx], self.joints, kp=self.kp, kd=self.kd, - velocities=self._data[self.idx:], forces=self.max_force) - else: - self.robot.set_joint_positions(data[:self.idx], self.joints, kp=self.kp, kd=self.kd, - velocities=data[self.idx:], forces=self.max_force) + def _write(self, data): + """apply the action data on the robot.""" + self.robot.set_joint_positions(data[:self.idx], self.joints, kp=self.kp, kd=self.kd, + velocities=data[self.idx:], forces=self.max_force) def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" @@ -190,12 +184,10 @@ class JointForceAction(JointAction): self.f_min = f_min self.f_max = f_max - def _write(self, data=None): - if data is None: - self.robot.set_joint_torques(self._data, self.joints) - else: - data = np.clip(data, self.f_min, self.f_max) - self.robot.set_joint_torques(data, self.joints) + def _write(self, data): + """apply the action data on the robot.""" + data = np.clip(data, self.f_min, self.f_max) + self.robot.set_joint_torques(data, self.joints) def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" @@ -232,12 +224,10 @@ class JointAccelerationAction(JointAction): self.a_min = a_min self.a_max = a_max - def _write(self, data=None): - if data is None: - self.robot.set_joint_accelerations(self._data, self.joints) - else: - data = np.clip(data, self.a_min, self.a_max) - self.robot.set_joint_accelerations(data, self.joints) + def _write(self, data): + """apply the action data on the robot.""" + data = np.clip(data, self.a_min, self.a_max) + self.robot.set_joint_accelerations(data, self.joints) def __copy__(self): """Return a shallow copy of the action. This can be overridden in the child class.""" diff --git a/pyrobolearn/actions/robot_actions/link_actions.py b/pyrobolearn/actions/robot_actions/link_actions.py index eca4d6f..15e2416 100644 --- a/pyrobolearn/actions/robot_actions/link_actions.py +++ b/pyrobolearn/actions/robot_actions/link_actions.py @@ -27,7 +27,7 @@ class LinkAction(RobotAction): def __init__(self, robot, link_ids=None): """ - Initialize the joint action. + Initialize the link action. Args: robot (Robot): robot instance @@ -62,17 +62,65 @@ class LinkAction(RobotAction): class LinkPositionAction(LinkAction): r"""Link position action - Set the link position(s) using IK. + Set the position using IK for the specified robot link(s). """ def __init__(self, robot, link_ids=None): + """ + Initialize the link position action. + + Args: + robot (Robot): robot instance + link_ids (int, int[N]): link id or list of link ids + """ super(LinkPositionAction, self).__init__(robot, link_ids) - def _write(self, data=None): - if data is None: - self.robot.set_link_positions(self.links, self._data) - else: - self.robot.set_link_positions(self.links, data) + def _write(self, data): + """apply the action data on the robot.""" + self.robot.set_link_positions(self.links, data) + + +class LinkVelocityAction(LinkAction): + r"""Link velocity action + + Set the cartesian velocity(ies) for the specified robot link(s). + """ + + def __init__(self, robot, link_ids=None): + """ + Initialize the link position action. + + Args: + robot (Robot): robot instance + link_ids (int, int[N]): link id or list of link ids + """ + super(LinkVelocityAction, self).__init__(robot, link_ids) + + def _write(self, data): + """apply the action data on the robot.""" + # self.robot + pass + + +class LinkForceAction(LinkAction): + r"""Link force action + + Set the cartesian force(s) for the specified robot link(s). + """ + def __init__(self, robot, link_ids=None): + """ + Initialize the link position action. + + Args: + robot (Robot): robot instance + link_ids (int, int[N]): link id or list of link ids + """ + super(LinkForceAction, self).__init__(robot, link_ids) + + def _write(self, data): + """apply the action data on the robot.""" + # self.robot + pass ########################