From 6fc9a0999384de595829de2378562a7e8dd890be Mon Sep 17 00:00:00 2001
From: Diego Ferigo <diego.ferigo@iit.it>
Date: Mon, 16 Sep 2019 18:27:10 +0200
Subject: [PATCH] Implemented new dummy methods in FactoryRobot

Waiting C++ support
---
 gym_ignition/robots/base/factory_robot.py | 60 ++++++++++++++++++-----
 1 file changed, 49 insertions(+), 11 deletions(-)

diff --git a/gym_ignition/robots/base/factory_robot.py b/gym_ignition/robots/base/factory_robot.py
index b22597ac5..b6bba6c72 100644
--- a/gym_ignition/robots/base/factory_robot.py
+++ b/gym_ignition/robots/base/factory_robot.py
@@ -2,14 +2,15 @@
 # This software may be modified and distributed under the terms of the
 # GNU Lesser General Public License v2.1 or any later version.
 
-from typing import List, Union
-from gym_ignition.base import robot
+import numpy as np
+from typing import List, Union, Tuple
 from gym_ignition.utils import logger
-from gym_ignition.base.robot import robot_joints
+from gym_ignition.base.robot import robot_abc, robot_joints
 from gym_ignition import gympp_bindings as bindings
 
 
-class FactoryRobot(robot.robot_abc.RobotABC):
+class FactoryRobot(robot_abc.RobotABC,
+                   robot_joints.RobotJoints):
     def __init__(self, robot_name: str, controller_rate: float = None) -> None:
         super().__init__()
 
@@ -39,12 +40,40 @@ def gympp_robot(self):
 
         return self._gympp_robot
 
+    # ========
+    # RobotABC
+    # ========
+
     def valid(self) -> bool:
         return self.gympp_robot.valid()
 
+    # ===========
+    # RobotJoints
+    # ===========
+
+    def dofs(self) -> None:
+        raise NotImplementedError
+
     def joint_names(self) -> List[str]:
         return self.gympp_robot.jointNames()
 
+    def joint_type(self, joint_name: str) -> robot_joints.JointType:
+        raise NotImplementedError
+
+    def joint_control_mode(self, joint_name: str) -> robot_joints.JointControlMode:
+        raise NotImplementedError
+
+    def set_joint_control_mode(self,
+                               joint_name: str,
+                               mode: robot_joints.JointControlMode) -> bool:
+        raise NotImplementedError
+
+    def initial_positions(self) -> np.ndarray:
+        raise NotImplementedError
+
+    def set_initial_positions(self, positions: np.ndarray) -> bool:
+        raise NotImplementedError
+
     def joint_position(self, joint_name: str) -> float:
         return self.gympp_robot.jointPosition(joint_name)
 
@@ -57,18 +86,15 @@ def joint_positions(self) -> List[float]:
     def joint_velocities(self) -> List[float]:
         return self.gympp_robot.jointVelocities()
 
-    def dt(self) -> float:
-        return self.gympp_robot.dt()
-
     def joint_pid(self, joint_name: str) -> Union[robot_joints.PID, None]:
         return self.gympp_robot.jointPID()
 
+    def dt(self) -> float:
+        return self.gympp_robot.dt()
+
     def set_dt(self, step_size: float) -> bool:
         return self.gympp_robot.setdt(step_size)
 
-    def set_joint_force(self, joint_name: str, force: float) -> bool:
-        return self.gympp_robot.setJointForce(joint_name, force)
-
     def set_joint_position_target(self, joint_name: str, position_reference: float) -> \
             bool:
         return self.gympp_robot.setJointPositionTarget(joint_name, position_reference)
@@ -77,6 +103,9 @@ def set_joint_velocity_target(self, joint_name: str, velocity_reference: float)
             bool:
         return self.gympp_robot.setJointVelocityTarget(joint_name, velocity_reference)
 
+    def set_joint_force(self, joint_name: str, force: float, clip: bool = False) -> bool:
+        return self.gympp_robot.setJointForce(joint_name, force)
+
     def set_joint_position(self, joint_name: str, position: float) -> bool:
         return self.gympp_robot.setJointPosition(joint_name, position)
 
@@ -86,8 +115,17 @@ def set_joint_velocity(self, joint_name: str, velocity: float) -> bool:
     def set_joint_pid(self, joint_name: str, pid: robot_joints.PID) -> bool:
         return self.gympp_robot.setJointPID(joint_name, pid)
 
-    def reset_joint(self, joint_name: str, position: float, velocity: float) -> bool:
+    def reset_joint(self,
+                    joint_name: str,
+                    position: float = None,
+                    velocity: float = None) -> bool:
         return self.gympp_robot.resetJoint(joint_name, position, velocity)
 
     def update(self, dt: float):
         return self.gympp_robot.update(dt)
+
+    def joint_position_limits(self, joint_name: str) -> Tuple[float, float]:
+        raise NotImplementedError
+
+    def joint_force_limit(self, joint_name: str) -> float:
+        raise NotImplementedError