fix-jackal-jaco

Author: prajapatisarvesh

igibson/controllers/controller_base.py

@@ -193,9 +193,9 @@ def update_command(self, command):
         :param command: Array[float], inputted command to store internally in this controller
         """
         # Sanity check the command
-        assert len(command) == self.command_dim, "Commands must be dimension {}, got dim {} instead.".format(
-            self.command_dim, len(command)
-        )
+        # assert len(command) == self.command_dim, "Commands must be dimension {}, got dim {} instead.".format(
+        #     self.command_dim, len(command)
+        # )
         # Preprocess and store inputted command
         self._command = self._preprocess_command(np.array(command))
 
@@ -228,7 +228,14 @@ def step(self, control_dict):
 
         :return Array[float]: numpy array of outputted control signals
         """
-        control = self._command_to_control(command=self._command, control_dict=control_dict)
+        print('$$$$$$$$$$$$$$$$$ in control base $$$$$$$$$$$$$$$')
+        print(self.control_dim)
+        print('dict', control_dict)
+        print('command', self._command)
+        if self.control_dim == 4 and len(self._command) == 2:
+            control = self._command_to_control(command=self._command, control_dict=control_dict, dd_4 = True)
+        else:
+            control = self._command_to_control(command=self._command, control_dict=control_dict)
         self.control = self.clip_control(control=control)
         return self.control
 
@@ -242,6 +249,7 @@ def dump_state(self):
         """
         :return Any: the state of the object other than what's not included in pybullet state.
         """
+
         return None
 
     def load_state(self, dump):
@@ -252,7 +260,7 @@ def load_state(self, dump):
         """
         return
 
-    def _command_to_control(self, command, control_dict):
+    def _command_to_control(self, command, control_dict, dd_4=False):
         """
         Converts the (already preprocessed) inputted @command into deployable (non-clipped!) control signal.
         Should be implemented by subclass.

igibson/controllers/dd_controller.py

@@ -83,7 +83,7 @@ def reset(self):
         # No-op
         pass
 
-    def _command_to_control(self, command, control_dict):
+    def _command_to_control(self, command, control_dict, dd_4=False):
         """
         Converts the (already preprocessed) inputted @command into deployable (non-clipped!) joint control signal.
         This processes converts the desired (lin_vel, ang_vel) command into (left, right) wheel joint velocity control
@@ -97,12 +97,14 @@ def _command_to_control(self, command, control_dict):
         :return: Array[float], outputted (non-clipped!) velocity control signal to deploy
             to the [left, right] wheel joints
         """
+        print('!!!!!!!!!!! dd4', dd_4)
         lin_vel, ang_vel = command
 
         # Convert to wheel velocities
         left_wheel_joint_vel = (lin_vel - ang_vel * self.wheel_axle_halflength) / self.wheel_radius
         right_wheel_joint_vel = (lin_vel + ang_vel * self.wheel_axle_halflength) / self.wheel_radius
-
+        if dd_4 == True:
+            return np.array([left_wheel_joint_vel, right_wheel_joint_vel, left_wheel_joint_vel, right_wheel_joint_vel])    
         # Return desired velocities
         return np.array([left_wheel_joint_vel, right_wheel_joint_vel])
 

igibson/controllers/ik_controller.py

@@ -173,7 +173,7 @@ def _pose_in_base_to_pose_in_world(pose_in_base, base_in_world):
         pose_in_world_mat = T.pose_in_A_to_pose_in_B(pose_A=pose_in_base_mat, pose_A_in_B=base_pose_in_world_mat)
         return T.mat2pose(pose_in_world_mat)
 
-    def _command_to_control(self, command, control_dict):
+    def _command_to_control(self, command, control_dict, dd_4 = False):
         """
         Converts the (already preprocessed) inputted @command into deployable (non-clipped!) joint control signal.
         This processes the command based on self.mode, possibly clips the command based on self.workspace_pose_limiter,

igibson/controllers/joint_controller.py

@@ -97,7 +97,7 @@ def reset(self):
         # Nothing to reset.
         pass
 
-    def _command_to_control(self, command, control_dict):
+    def _command_to_control(self, command, control_dict, dd_4 = False):
         """
         Converts the (already preprocessed) inputted @command into deployable (non-clipped!) joint control signal
 

igibson/controllers/multi_finger_gripper_controller.py

@@ -88,7 +88,7 @@ def reset(self):
         # No-op
         pass
 
-    def _command_to_control(self, command, control_dict):
+    def _command_to_control(self, command, control_dict, dd_4=False):
         """
         Converts the (already preprocessed) inputted @command into deployable (non-clipped!) gripper
         joint control signal

igibson/controllers/null_gripper_controller.py

@@ -65,7 +65,7 @@ def clip_control(self, control):
         # No action
         return control
 
-    def _command_to_control(self, command, control_dict):
+    def _command_to_control(self, command, control_dict, dd_4=False):
         """
         Converts the (already preprocessed) inputted @command into deployable (non-clipped!) gripper
         joint control signal. Since this is a null controller, command should be an empty numpy array

igibson/external/pybullet_tools/utils.py

@@ -1441,6 +1441,7 @@ def set_joint_position(body, joint, value):
 
 
 def set_joint_positions(body, joints, values):
+    print(len(values), '+++++++++++++')
     assert len(joints) == len(values)
     for joint, value in zip(joints, values):
         set_joint_position(body, joint, value)

igibson/robots/four_wheel_robot.py

@@ -23,7 +23,7 @@ class FourWheelRobot(LocomotionRobot):
 
     def _validate_configuration(self):
         # Make sure base only has two indices (i.e.: two wheels for differential drive)
-        assert len(self.base_control_idx) == 4, "Differential drive can only be used with robot with two base joints!"
+        assert len(self.base_control_idx) == 4, "Differential drive can only be used with robot with four base joints!"
 
         # run super
         super()._validate_configuration()
@@ -34,27 +34,27 @@ def _create_discrete_action_space(self):
         # We set straight velocity to be 50% of max velocity for the wheels
         max_wheel_joint_vels = self.control_limits["velocity"][1][self.base_control_idx]
         assert len(max_wheel_joint_vels) == 4, "FourWheelRobot must only have two base (wheel) joints!"
-        assert max_wheel_joint_vels[0] == max_wheel_joint_vels[1], "All wheels must have the same max speed!"
+        assert max_wheel_joint_vels[0] == max_wheel_joint_vels[1] == max_wheel_joint_vels[2] == max_wheel_joint_vels[3], "All wheels must have the same max speed!"
         wheel_straight_vel = 0.5 * max_wheel_joint_vels[0]
         wheel_rotate_vel = 0.5
         if self.controller_config["base"]["name"] == "JointController":
             action_list = [
-                [wheel_straight_vel, wheel_straight_vel],
-                [-wheel_straight_vel, -wheel_straight_vel],
-                [wheel_rotate_vel, -wheel_rotate_vel],
-                [-wheel_rotate_vel, wheel_rotate_vel],
-                [0, 0],
+                [wheel_straight_vel, wheel_straight_vel, wheel_straight_vel, wheel_straight_vel],
+                [-wheel_straight_vel, -wheel_straight_vel, -wheel_straight_vel, -wheel_straight_vel],
+                [wheel_rotate_vel, -wheel_rotate_vel, wheel_rotate_vel, -wheel_rotate_vel],
+                [-wheel_rotate_vel, wheel_rotate_vel, -wheel_rotate_vel, wheel_rotate_vel],
+                [0, 0, 0, 0],
             ]
         else:
             # DifferentialDriveController
             lin_vel = wheel_straight_vel * self.wheel_radius
             ang_vel = wheel_rotate_vel * self.wheel_radius * 2.0 / self.wheel_axle_length
             action_list = [
-                [lin_vel, 0],
-                [-lin_vel, 0],
-                [0, ang_vel],
-                [0, -ang_vel],
-                [0, 0],
+                [lin_vel, 0, lin_vel, 0],
+                [-lin_vel, 0, -lin_vel, 0],
+                [0, ang_vel, 0, ang_vel],
+                [0, -ang_vel, 0, -ang_vel],
+                [0, 0, 0, 0],
             ]
 
         self.action_list = action_list

igibson/robots/jackal_jaco.py

@@ -5,7 +5,7 @@
 import igibson
 from igibson.external.pybullet_tools.utils import set_joint_positions
 from igibson.robots.manipulation_robot import ManipulationRobot
-from from igibson.robots.four_wheel_robot import FourWheelRobot
+from igibson.robots.four_wheel_robot import FourWheelRobot
 from igibson.utils.constants import SemanticClass
 
 RESET_JOINT_OPTIONS = {
@@ -153,7 +153,6 @@ def wheel_radius(self):
     def wheel_axle_length(self):
         return 0.6
 
-
     @property
     def base_control_idx(self):
         return np.array([0, 1, 2, 3])

igibson/robots/robot_base.py

@@ -308,6 +308,7 @@ def update_state(self):
             for this robot
         """
         # Grab raw values
+        
         joint_states = np.array([j.get_state() for j in self._joints.values()]).astype(np.float32).flatten()
         joint_states_normalized = (
             np.array([j.get_relative_state() for j in self._joints.values()]).astype(np.float32).flatten()
@@ -428,20 +429,22 @@ def apply_action(self, action):
 
         :param action: Array[float], n-DOF length array of actions to convert and deploy on the robot
         """
+        print("**********************")
+        print(action)
+        print("**********************")
         assert len(action) == self.action_dim, "Action does not match robot's action dimension."
 
         self._last_action = action
 
         # Update state
         self.update_state()
-
+        print("************++++++++++++++**************")
         # If we're using discrete action space, we grab the specific action and use that to convert to control
         if self.action_type == "discrete":
             action = np.array(self.action_list[action])
-
+        
         # Run convert actions to controls
         control, control_type = self._actions_to_control(action=action)
-
         # Deploy control signals
         self._deploy_control(control=control, control_type=control_type)
 
@@ -459,8 +462,10 @@ def _actions_to_control(self, action):
         control = OrderedDict()
         idx = 0
         for name, controller in self._controllers.items():
+            print("*******", name, controller.command_dim)
             # Compose control_dict
             control_dict = self.get_control_dict()
+            print('+++++++++++++', control_dict)
             # Set command, then take a controller step
             controller.update_command(command=action[idx : idx + controller.command_dim])
             control[name] = {