update simulators

pyrobolearn/simulators/bullet.py

@@ -91,8 +91,8 @@ class Bullet(Simulator):
         Args:
             render (bool): if True, it will open the GUI, otherwise, it will just run the server.
             num_instances (int): number of simulator instances.
-            **kwargs (dict): optional arguments (this is not used here).
             middleware (MiddleWare, None): middleware instance.
+            **kwargs (dict): optional arguments (this is not used here).
         """
         # try to import the pybullet library
         # normally that should be done outside the class but because it might have some conflicts with other libraries
@@ -1616,7 +1616,7 @@ class Bullet(Simulator):
             states[idx][2] = np.asarray(state[2])
         return states
 
-    def reset_joint_state(self, body_id, joint_id, position, velocity=0.):
+    def reset_joint_state(self, body_id, joint_id, position, velocity=None):
         """
         Reset the state of the joint. It is best only to do this at the start, while not running the simulation:
         `reset_joint_state` overrides all physics simulation. Note that we only support 1-DOF motorized joints at
@@ -1628,7 +1628,10 @@ class Bullet(Simulator):
             position (float): the joint position (angle in radians [rad] or position [m])
             velocity (float): the joint velocity (angular [rad/s] or linear velocity [m/s])
         """
-        self.sim.resetJointState(body_id, joint_id, position, velocity)
+        if velocity is None:
+            self.sim.resetJointState(body_id, joint_id, position)
+        else:
+            self.sim.resetJointState(body_id, joint_id, position, velocity)
 
     def enable_joint_force_torque_sensor(self, body_id, joint_ids, enable=True):
         """

pyrobolearn/simulators/simulator.py

@@ -1219,7 +1219,7 @@ class Simulator(object):
         """
         pass
 
-    def reset_joint_state(self, body_id, joint_id, position, velocity=0.):
+    def reset_joint_state(self, body_id, joint_id, position, velocity=None):
         """
         Reset the state of the joint. It is best only to do this at the start, while not running the simulation:
         `reset_joint_state` overrides all physics simulation.
@@ -1467,6 +1467,38 @@ class Simulator(object):
     def get_link_velocities(self, body_id, link_ids):
         pass
 
+    def get_link_world_linear_accelerations(self, body_id, link_ids):
+        """
+        Return the linear acceleration of the link(s) expressed in the Cartesian world space coordinates.
+
+        Args:
+            body_id (int): unique body id.
+            link_ids (int, list[int]): link index, or list of link indices.
+
+        Returns:
+            if 1 link:
+                np.array[float[3]]: linear acceleration of the link in the Cartesian world space
+            if multiple links:
+                np.array[float[N,3]]: linear acceleration of each link
+        """
+        pass
+
+    def get_link_world_angular_accelerations(self, body_id, link_ids):
+        """
+        Return the angular acceleration of the link(s) in the Cartesian world space coordinates.
+
+        Args:
+            body_id (int): unique body id.
+            link_ids (int, list[int]): link index, or list of link indices.
+
+        Returns:
+            if 1 link:
+                np.array[float[3]]: angular acceleration of the link in the Cartesian world space
+            if multiple links:
+                np.array[float[N,3]]: angular acceleration of each link
+        """
+        pass
+
     def get_link_world_accelerations(self, body_id, link_ids):
         """
         Return the linear and angular accelerations (expressed in the Cartesian world space coordinates) for the given

pyrobolearn/utils/parsers/robots/data_structures.py

@@ -822,10 +822,7 @@ class MultiBody(object):
     @property
     def num_dofs(self):
         """Return the total number of degrees of freedom."""
-        num_dofs = 0
-        for joint in self.joints.values():
-            num_dofs += joint.num_dofs
-        return num_dofs
+        return sum([joint.num_dofs for joint in self.joints.values()])
 
     @property
     def num_bodies(self):
@@ -834,23 +831,21 @@ class MultiBody(object):
 
     @property
     def num_joints(self):
-        """Return the total number of joints in this multi-body (this accounts for fixed joints as well, but not
-        free joints)."""
+        """Return the total number of joints which are not free joints (so this accounts for fixed joints as well, but
+        not free joints)."""
         # return len(self.joints)
-        num_joints = 0
-        for joint in self.joints.values():
-            if joint.dtype != 'floating':
-                num_joints += 1
-        return num_joints
+        return sum([1 for joint in self.joints.values() if joint.dtype != 'floating'])
+
+    @property
+    def num_free_joints(self):
+        """Return the total number of free joints (this does not include the fixed joints). Basically it is the joints
+        that have at least 1 DoF."""
+        return sum([1 for joint in self.joints.values() if joint.dtype != 'fixed'])
 
     @property
     def num_actuated_joints(self):
         """Return the total number of joints which are not fixed nor free."""
-        num_actuated_joints = 0
-        for joint in self.joints.values():
-            if joint.dtype != 'fixed' and joint.dtype != 'floating':
-                num_actuated_joints += 1
-        return num_actuated_joints
+        return sum([1 for joint in self.joints.values() if joint.dtype != 'fixed' and joint.dtype != 'floating'])
 
     @property
     def root(self):
@@ -873,6 +868,11 @@ class MultiBody(object):
         """Return if the root element in the tree is static or not."""
         if self.root is not None:
             return self.root.static
+        if self.joints:
+            joint = self.joints[next(iter(self.joints))]
+            if joint.dtype == 'free' or joint.dtype == 'floating':
+                return False
+            return True
 
     @static.setter
     def static(self, static):
@@ -880,6 +880,10 @@ class MultiBody(object):
         if self.root is not None:
             self.root.static = static
 
+    # aliases
+    fixed = static
+    fixed_base = static
+
     @property
     def position(self):
         """Return the tree frame position."""
@@ -963,15 +967,25 @@ class MultiBody(object):
         if not isinstance(joint, Joint):
             raise TypeError("Expecting the given 'joint' to be an instance of `Joint`, but got instead: "
                             "{}".format(type(joint)))
-        if idx is not None:
+
+        if idx is None:
             self.joints[joint.name] = joint
         else:
-            # this insert
-            joints = OrderedDict()
-            for i, (joint_name, joint_instance) in enumerate(self.joints.items()):
-                if i == idx:
+            # inserts a joint at the specified index
+            if idx == 0 and len(self.joints) == 0:  # first joint ever to insert
+                self.joints[joint.name] = joint
+            else:
+                joints = OrderedDict()
+                for i, (joint_name, joint_instance) in enumerate(self.joints.items()):
+                    if i == idx:
+                        joints[joint.name] = joint
+                    joints[joint_name] = joint_instance
+
+                if idx == len(self.joints):  # last joint
                     joints[joint.name] = joint
-                joint[joint_name] = joint_instance
+
+                # replace old joint dictionary
+                self.joints = joints
 
 
 # alias