few corrections

examples/dynamics/control/attractor_point.py

@@ -22,6 +22,7 @@ robot = prl.robots.RRBot(sim)
 robot.disable_motor()           # disable motors; comment the `robot.set_joint_torques(torques)` to see what happens
 robot.print_info()
 robot.change_transparency()
+world.load_robot(robot)
 
 # define variables
 link_id = robot.get_link_ids('hokuyo_link')         # the link we are interested to

examples/dynamics/control/gravity_compensation.py

@@ -22,6 +22,7 @@ robot = prl.robots.RRBot(sim)
 robot.disable_motor()       # disable motors; comment the `robot.set_joint_torques(torques)` to see what happens
 robot.print_info()
 robot.change_transparency()
+world.load_robot(robot)
 
 
 # run simulator

examples/dynamics/control/no_forces.py

@@ -22,6 +22,7 @@ robot = prl.robots.RRBot(sim)
 robot.disable_motor()       # disable motors
 robot.print_info()
 robot.change_transparency()
+world.load_robot(robot)
 
 
 # run simulator

examples/kinematics/inverse/moving_sphere.py

@@ -19,6 +19,7 @@ world = BasicWorld(sim)
 # create robot
 robot = KukaIIWA(sim)
 robot.print_info()
+world.load_robot(robot)
 
 # define useful variables for IK
 dt = 1./240
@@ -61,7 +62,7 @@ for t in count():
     else:
         J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx]
 
-    # Pseudo-inverse
+    # Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
     Jp = robot.get_damped_least_squares_inverse(J, damping)
 
     # evaluate damped-least-squares IK
@@ -72,5 +73,4 @@ for t in count():
     robot.set_joint_positions(q, joint_ids=joint_ids)
 
     # step in simulation
-    robot.step()
     world.step(sleep_dt=dt)

pyrobolearn/robots/__init__.py

@@ -176,7 +176,7 @@ implemented_grippers = set(implemented_grippers)
 
 # function to disable the motors
 # this can be useful when resetting the joint state
-def reset_robot(robot, joint_ids=None):
+def disable_motors(robot, joint_ids=None):
     """Return a function that disables the motors."""
     def reset():
         robot.disable_motor(joint_ids=joint_ids)

pyrobolearn/robots/robot.py

@@ -1638,8 +1638,9 @@ class Robot(ControllableBody):
             if multiple links: list of above
         """
         if isinstance(link_ids, int):
-            return self.get_link_states(self, link_ids, False, False)[2]
-        return [state[2] for state in self.get_link_states(self, link_ids, False, False)]
+            return self.get_link_states(link_ids, compute_link_velocity=False, compute_forward_kinematics=False)[2]
+        return [state[2] for state in self.get_link_states(link_ids, compute_link_velocity=False,
+                                                           compute_forward_kinematics=False)]
 
     def get_link_local_orientations(self, link_ids=None):
         """

pyrobolearn/simulators/rbdl_.py

@@ -50,7 +50,7 @@ class RBDL(object):
             floating_base (bool): if True, the model will be considered to have a floating base, thus 6 more DoFs will
                 be added.
         """
-        self.model = rbdl.loadModel(filename, verbose=verbose, floating_base=floating_base)
+        self.model = rbdl.loadModel(filename.encode(), verbose=verbose, floating_base=floating_base)
 
     # alias
     load_urdf = load_model
@@ -107,7 +107,7 @@ if __name__ == '__main__':
     import os
     import pyrobolearn as prl
 
-    fixed_base = False
+    fixed_base = True
 
     robot = prl.robots.HyQ2Max(prl.simulators.Bullet(render=False), fixed_base=fixed_base)
     rbdl_ = RBDL(os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/hyq2max/hyq2max.urdf',
@@ -127,7 +127,7 @@ if __name__ == '__main__':
     print_attr("Num of joints: {}", 'num_joints')
     print_attr("Num of links: {}", 'num_links')
 
-    print(robot.get_link_names([-1] + range(robot.num_links)))
+    print(robot.get_link_names([-1] + list(range(robot.num_links))))
     print(rbdl_.get_link_names(range(rbdl_.num_links)))
 
     print("Num fixed links: {}".format(robot.num_links - robot.num_actuated_joints))

pyrobolearn/worlds/world.py

@@ -733,16 +733,18 @@ class World(object):
         """
         return self.sim.get_visual_shape_data(body_id)[-1]
 
-    def change_body_color(self, body_id, color, link_id=-1):
+    def change_body_color(self, body, color, link_id=-1):
         """
         Change the color of the given body.
 
         Args:
-            body_id (int, Body): body (id)
+            body (int, Body): body (id)
             color (float[4]): RGBA color where each channel is between 0 and 1.
             link_id (int): link id
         """
-        self.sim.change_visual_shape(body_id, link_id, rgba_color=color)
+        if isinstance(body, Body):
+            body = body.id
+        self.sim.change_visual_shape(body, link_id, rgba_color=color)
 
     def get_body_position(self, body_id):
         """

scripts/install_rbdl.sh

@@ -60,15 +60,69 @@ sudo cp rbdl.so /usr/local/lib/python2.7/site-packages
 cd currentdir
 
 
-## to wrap GetBodyId() function
+## to wrap GetBodyId(), GetBodyName() function
 # 
-# Inside crbdl.pxd, add:
+# Inside crbdl.pxd,
+# - under `cdef cppclass Model`, add:
+#
 # unsigned int GetBodyId(const char *body_name)
-# inside cdef cppclass Model:
-# 
-# Inside rbdl-wrapper.pyx, add:
+#
+# string GetBodyName(unsigned int body_id)
+#
+# - under `cdef extern from "<rbdl/Kinematics.h>" namespace "RigidBodyDynamics":`, add:
+#
+# cdef void UpdateKinematics (Model& model,
+#            const VectorNd &q,
+#            const VectorNd &qdot,
+#            const VectorNd &qddot)
+#
+# cdef Matrix3d CalcBodyWorldOrientation (Model& model,
+#            const VectorNd &q,
+#            const unsigned int body_id,
+#            bool update_kinematics)
+#
+# Inside rbdl-wrapper.pyx,
+# - under `cdef class Model`, add:
+#
 # def GetBodyId (self, char* body_name):
 #      return self.thisptr.GetBodyId(body_name)
-# inside cdef class Model:
-# 
+#
+# def GetBodyName(self, unsigned int index):
+#      return self.thisptr.GetBodyName(index)
+#
+# - under `kinematics.h` comment block, add:
+#
+# def UpdateKinematics(
+#        Model model,
+#        np.ndarray[double, ndim=1, mode="c"] q,
+#        np.ndarray[double, ndim=1, mode="c"] qdot,
+#        np.ndarray[double, ndim=1, mode="c"] qddot
+#):
+#    crbdl.UpdateKinematics(
+#            model.thisptr[0],
+#            NumpyToVectorNd (q),
+#            NumpyToVectorNd (qdot),
+#            NumpyToVectorNd (qddot)
+#    )
+#
+# def CalcBodyWorldOrientation (Model model,
+#        np.ndarray[double, ndim=1, mode="c"] q,
+#        unsigned int body_id,
+#        update_kinematics=True):
+#    return Matrix3dToNumpy (crbdl.CalcBodyWorldOrientation (
+#            model.thisptr[0],
+#            NumpyToVectorNd (q),
+#            body_id,
+#            update_kinematics
+#            ))
+#
+# - under `Conversion Numpy <-> Eigen` comment block, add:
+#
+# cdef np.ndarray Matrix3dToNumpy (crbdl.Matrix3d cM):
+#    result = np.ndarray ([3, 3])
+#    for i in range (3):
+#        for j in range (3):
+#            result[i,j] = cM.coeff(i,j)
+#    return result
+#
 # then do cmake, make, install again