Remove remaining joints, fix actuator computation and restrict qpos r…

…andom init

robosuite/models/robots/manipulators/legged_manipulator_model.py

@@ -35,6 +35,12 @@ def _remove_joint_actuation(self, part_name):
                 parent_body = find_parent(self.actuator, motor)
                 parent_body.remove(motor)
                 self._actuators.remove(motor.get("name").replace(self.naming_prefix, ""))
+        for fixed in self.tendon.findall(".//fixed"):
+            for joint in fixed.findall(".//joint"):
+                if part_name in joint.get("joint"):
+                    parent_body = find_parent(self.tendon, fixed)
+                    parent_body.remove(fixed)
+                    break
 
     def _remove_free_joint(self):
         # remove freejoint

robosuite/robots/legged_robot.py

@@ -184,11 +184,15 @@ def control(self, action, policy_step=False):
 
         applied_action_dict = self.composite_controller.run_controller(self._enabled_parts)
         for part_name, applied_action in applied_action_dict.items():
-            applied_action_low = self.sim.model.actuator_ctrlrange[self._ref_actuators_indexes_dict[part_name], 0]
-            applied_action_high = self.sim.model.actuator_ctrlrange[self._ref_actuators_indexes_dict[part_name], 1]
-            applied_action = np.clip(applied_action, applied_action_low, applied_action_high)
-
-            self.sim.data.ctrl[self._ref_actuators_indexes_dict[part_name]] = applied_action
+            joint_id2action = {}
+            for i, joint_id in enumerate(self._ref_joints_indexes_dict[part_name]):
+                joint_id2action[joint_id] = applied_action[i]
+            for actuator_id in self._ref_actuators_indexes_dict[part_name]:
+                joint_id = self.sim.model.actuator_trnid[actuator_id][0]
+                action = joint_id2action[joint_id] / self.sim.model.actuator_gear[actuator_id][0]
+                low = self.sim.model.actuator_ctrlrange[actuator_id, 0]
+                high = self.sim.model.actuator_ctrlrange[actuator_id, 1]
+                self.sim.data.ctrl[actuator_id] = np.clip(action, low, high)
 
         # If this is a policy step, also update buffers holding recent values of interest
         if policy_step:

robosuite/robots/robot.py

@@ -253,6 +253,10 @@ def reset(self, deterministic=False):
                 raise ValueError("Error: Invalid noise type specified. Options are 'gaussian' or 'uniform'.")
             init_qpos += noise
 
+        low_qpos = self.sim.model.jnt_range[self._ref_joint_pos_indexes, 0]
+        high_qpos = self.sim.model.jnt_range[self._ref_joint_pos_indexes, 1]
+        init_qpos = np.clip(init_qpos, low_qpos, high_qpos)
+
         # Set initial position in sim
         self.sim.data.qpos[self._ref_joint_pos_indexes] = init_qpos