diff --git a/examples/dynamics/control/attractor_point.py b/examples/dynamics/control/attractor_point.py index 794672d..48baf08 100644 --- a/examples/dynamics/control/attractor_point.py +++ b/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 diff --git a/examples/dynamics/control/gravity_compensation.py b/examples/dynamics/control/gravity_compensation.py index 6d13738..6c573d2 100644 --- a/examples/dynamics/control/gravity_compensation.py +++ b/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 diff --git a/examples/dynamics/control/no_forces.py b/examples/dynamics/control/no_forces.py index 24dd125..ce4ffa2 100644 --- a/examples/dynamics/control/no_forces.py +++ b/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 diff --git a/examples/kinematics/inverse/moving_sphere.py b/examples/kinematics/inverse/moving_sphere.py index 12fa2e8..97c3b9a 100644 --- a/examples/kinematics/inverse/moving_sphere.py +++ b/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) diff --git a/pyrobolearn/robots/__init__.py b/pyrobolearn/robots/__init__.py index 203c272..22ee5e0 100644 --- a/pyrobolearn/robots/__init__.py +++ b/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) diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index f1e1adf..46faaa1 100644 --- a/pyrobolearn/robots/robot.py +++ b/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): """ diff --git a/pyrobolearn/simulators/rbdl_.py b/pyrobolearn/simulators/rbdl_.py index 2dc9442..c9e7b7d 100644 --- a/pyrobolearn/simulators/rbdl_.py +++ b/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)) diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index f27dc19..e054576 100644 --- a/pyrobolearn/worlds/world.py +++ b/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): """ diff --git a/scripts/install_rbdl.sh b/scripts/install_rbdl.sh index 914df19..2cda0a2 100755 --- a/scripts/install_rbdl.sh +++ b/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 "" 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