From cd03a1f85f5c8fc55efd2a84c3f056e911388a32 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Wed, 18 Sep 2019 22:47:39 +0200 Subject: [PATCH] update simulators and ROS middleware integration --- pyrobolearn/robots/robot.py | 16 +- pyrobolearn/robots/urdfs/rrbot/rrbot.yaml | 15 + pyrobolearn/simulators/__init__.py | 23 +- pyrobolearn/simulators/bullet.py | 22 +- pyrobolearn/simulators/bullet_ros.py | 34 +- .../simulators/middlewares/__init__.py | 14 + .../simulators/middlewares/middleware.py | 179 +++ pyrobolearn/simulators/middlewares/ros.py | 764 ++++++++- .../simulators/middlewares/ros_publisher.py | 152 ++ .../simulators/middlewares/ros_subscriber.py | 154 ++ pyrobolearn/simulators/mujoco.py | 891 ++++++++++- pyrobolearn/simulators/raisim.py | 1381 ++++++++++++++++- pyrobolearn/simulators/simulator.py | 168 +- pyrobolearn/utils/mesh.py | 11 +- .../utils/parsers/robots/data_structures.py | 412 ++++- .../utils/parsers/robots/mujoco_parser.py | 11 +- .../utils/parsers/robots/robot_parser.py | 1 + .../utils/parsers/robots/urdf_parser.py | 20 +- 18 files changed, 4068 insertions(+), 200 deletions(-) create mode 100644 pyrobolearn/robots/urdfs/rrbot/rrbot.yaml create mode 100644 pyrobolearn/simulators/middlewares/ros_publisher.py create mode 100644 pyrobolearn/simulators/middlewares/ros_subscriber.py diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py index ce1c009..cec41f2 100644 --- a/pyrobolearn/robots/robot.py +++ b/pyrobolearn/robots/robot.py @@ -1629,10 +1629,10 @@ class Robot(ControllableBody): Args: link_ids (int, int[N], None): link id, or list of desired link ids. If None, get the state of all links - associated to actuated joints. + associated to actuated joints. compute_link_velocity (bool): if True, the Cartesian world velocity will be computed and returned. compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed - using forward kinematics. + using forward kinematics. Returns: if 1 link: @@ -2696,9 +2696,9 @@ class Robot(ControllableBody): Args: link_id (int): link id. q (np.array[float[N]], None): joint positions of size N, where N is the number of DoFs. If None, it will - compute q based on the current joint positions. + compute q based on the current joint positions. local_position (None, np.array[float[3]]): the point on the specified link to compute the Jacobian (in link - local coordinates around its center of mass). If None, it will use the CoM position (in the link frame). + local coordinates around its center of mass). If None, it will use the CoM position (in the link frame). Returns: np.array[float[6,N]], np.array[float[6,6+N]]: full geometric (linear and angular) Jacobian matrix. The @@ -2737,9 +2737,9 @@ class Robot(ControllableBody): Args: link_id (int): link id q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs. If None, it will compute - q based on the current joint positions. + q based on the current joint positions. local_position: the point on the specified link to compute the Jacobian (in link local coordinates around - its center of mass). If None, it will use the CoM position (in the link frame). + its center of mass). If None, it will use the CoM position (in the link frame). Returns: np.array[float[3,N]], np.array[float[3,6+N]]: full linear geometric Jacobian matrix. The number of @@ -2761,9 +2761,9 @@ class Robot(ControllableBody): Args: link_id (int): link id q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs. If None, it will compute - q based on the current joint positions. + q based on the current joint positions. local_position: the point on the specified link to compute the Jacobian (in link local coordinates around - its center of mass). If None, it will use the CoM position (in the link frame). + its center of mass). If None, it will use the CoM position (in the link frame). Returns: np.array[float[3,N]], np.array[float[3,6+N]]: full angular geometric Jacobian matrix. The number of diff --git a/pyrobolearn/robots/urdfs/rrbot/rrbot.yaml b/pyrobolearn/robots/urdfs/rrbot/rrbot.yaml new file mode 100644 index 0000000..9cbcf45 --- /dev/null +++ b/pyrobolearn/robots/urdfs/rrbot/rrbot.yaml @@ -0,0 +1,15 @@ +rrbot: + # Publish all joint states ----------------------------------- + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + # Position Controllers --------------------------------------- + joint1_position_controller: + type: effort_controllers/JointPositionController + joint: joint1 + pid: {p: 100.0, i: 0.01, d: 10.0} + joint2_position_controller: + type: effort_controllers/JointPositionController + joint: joint2 + pid: {p: 100.0, i: 0.01, d: 10.0} diff --git a/pyrobolearn/simulators/__init__.py b/pyrobolearn/simulators/__init__.py index b06fa53..478d963 100644 --- a/pyrobolearn/simulators/__init__.py +++ b/pyrobolearn/simulators/__init__.py @@ -2,6 +2,9 @@ # load middlewares from . import middlewares +# check Python version +import sys +python_version = sys.version_info[0] # load all simulators @@ -14,14 +17,24 @@ from .bullet import Bullet # Bullet ros simulator from .bullet_ros import BulletROS -# Dart simulator -# from .dart import Dart +if python_version >= 3: + # Dart simulator + try: + from .dart import Dart + except ImportError as e: + print("Dart not found.") -# MuJoCo simulator -# from .mujoco import Mujoco + # MuJoCo simulator + try: + from .mujoco import Mujoco + except ImportError as e: + print("MuJoCo not found.") # Raisim simulator -# from .raisim import Raisim +try: + from .raisim import Raisim +except ImportError as e: + print("Raisim not found.") # Vrep simulator (note that there is a currently a problem when loading pybullet with pyrep) # from .vrep import VREP diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 9c43b35..ec9741e 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -1735,15 +1735,15 @@ class Bullet(Simulator): link_id (int): link index. compute_velocity (bool): If True, the Cartesian world velocity will be computed and returned. compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed - using forward kinematics. + using forward kinematics. Returns: np.array[float[3]]: Cartesian world position of CoM np.array[float[4]]: Cartesian world orientation of CoM, in quaternion [x,y,z,w] np.array[float[3]]: local position offset of inertial frame (center of mass) expressed in the URDF - link frame + link frame np.array[float[4]]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in URDF - link frame + link frame np.array[float[3]]: world position of the URDF link frame np.array[float[4]]: world orientation of the URDF link frame (expressed as a quaternion [x,y,z,w]) np.array[float[3]]: Cartesian world linear velocity. Only returned if `compute_velocity` is True. @@ -2214,7 +2214,7 @@ class Bullet(Simulator): return np.asarray(self.sim.getJointInfo(body_id, joint_ids)[-4]) return np.asarray([self.sim.getJointInfo(body_id, joint_id)[-4] for joint_id in joint_ids]) - def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None): + def _set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None): """ Set the position of the given joint(s) (using position control). @@ -2230,7 +2230,7 @@ class Bullet(Simulator): self.set_joint_motor_control(body_id, joint_ids, control_mode=pybullet.POSITION_CONTROL, positions=positions, velocities=velocities, forces=forces, kp=kps, kd=kds) - def get_joint_positions(self, body_id, joint_ids): + def _get_joint_positions(self, body_id, joint_ids): """ Get the position of the given joint(s). @@ -2248,7 +2248,7 @@ class Bullet(Simulator): return self.sim.getJointState(body_id, joint_ids)[0] return np.asarray([state[0] for state in self.sim.getJointStates(body_id, joint_ids)]) - def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None): + def _set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None): """ Set the velocity of the given joint(s) (using velocity control). @@ -2269,7 +2269,7 @@ class Bullet(Simulator): self.sim.setJointMotorControlArray(body_id, joint_ids, self.sim.VELOCITY_CONTROL, targetVelocities=velocities, forces=max_force) - def get_joint_velocities(self, body_id, joint_ids): + def _get_joint_velocities(self, body_id, joint_ids): """ Get the velocity of the given joint(s). @@ -2372,7 +2372,7 @@ class Bullet(Simulator): # q_idx = self.get_q_indices(body_id, joint_ids) # return accelerations[q_idx] - def set_joint_torques(self, body_id, joint_ids, torques): + def _set_joint_torques(self, body_id, joint_ids, torques): """ Set the torque/force to the given joint(s) (using force/torque control). @@ -2385,7 +2385,7 @@ class Bullet(Simulator): self.sim.setJointMotorControl2(body_id, joint_ids, self.sim.TORQUE_CONTROL, force=torques) self.sim.setJointMotorControlArray(body_id, joint_ids, self.sim.TORQUE_CONTROL, forces=torques) - def get_joint_torques(self, body_id, joint_ids): + def _get_joint_torques(self, body_id, joint_ids): """ Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`. Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the @@ -3397,8 +3397,8 @@ class Bullet(Simulator): Args: body_id (int): unique body id. link_id (int): link id. - local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link local - coordinates around its center of mass). If None, it will use the CoM position (in the link frame). + local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link + local coordinates around its center of mass). If None, it will use the CoM position (in the link frame). q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs. dq (np.array[float[N]]): joint velocities of size N, where N is the number of DoFs. des_ddq (np.array[float[N]]): desired joint accelerations of size N. diff --git a/pyrobolearn/simulators/bullet_ros.py b/pyrobolearn/simulators/bullet_ros.py index df002eb..933e66e 100644 --- a/pyrobolearn/simulators/bullet_ros.py +++ b/pyrobolearn/simulators/bullet_ros.py @@ -231,13 +231,13 @@ class BulletROS(Bullet): # , ROS): Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. + joint_ids (int, list[int]): joint id, or list of joint ids. Returns: if 1 joint: float: joint position [rad] if multiple joints: - np.float[N]: joint positions [rad] + np.array[float[N]]: joint positions [rad] """ if body_id in self.subscribers: q = self.subscribers[body_id].get_joint_positions(joint_ids) @@ -258,12 +258,12 @@ class BulletROS(Bullet): # , ROS): Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. - positions (float, np.float[N]): desired position, or list of desired positions [rad] - velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s] - kps (None, float, np.float[N]): position gain(s) - kds (None, float, np.float[N]): velocity gain(s) - forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values. + joint_ids (int, list[int]): joint id, or list of joint ids. + positions (float, np.array[float[N]]): desired position, or list of desired positions [rad] + velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + kps (None, float, np.array[float[N]]): position gain(s) + kds (None, float, np.array[float[N]]): velocity gain(s) + forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values. """ super(BulletROS, self).set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces) if body_id in self.publishers: @@ -276,13 +276,13 @@ class BulletROS(Bullet): # , ROS): Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. + joint_ids (int, list[int]): joint id, or list of joint ids. Returns: if 1 joint: float: joint velocity [rad/s] if multiple joints: - np.float[N]: joint velocities [rad/s] + np.array[float[N]]: joint velocities [rad/s] """ if body_id in self.subscribers: dq = self.subscribers[body_id].get_joint_velocities(joint_ids) @@ -303,9 +303,9 @@ class BulletROS(Bullet): # , ROS): Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. - velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s] - max_force (None, float, np.float[N]): maximum motor forces/torques + joint_ids (int, list[int]): joint id, or list of joint ids. + velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + max_force (None, float, np.array[float[N]]): maximum motor forces/torques """ super(BulletROS, self).set_joint_velocities(body_id, joint_ids, velocities, max_force) if body_id in self.publishers: @@ -320,13 +320,13 @@ class BulletROS(Bullet): # , ROS): Args: body_id (int): unique body id. - joint_ids (int, list of int): a joint id, or list of joint ids. + joint_ids (int, list[int]): a joint id, or list of joint ids. Returns: if 1 joint: float: torque [Nm] if multiple joints: - np.float[N]: torques associated to the given joints [Nm] + np.array[float[N]]: torques associated to the given joints [Nm] """ if body_id in self.subscribers: torques = self.subscribers[body_id].get_joint_torques(joint_ids) @@ -347,8 +347,8 @@ class BulletROS(Bullet): # , ROS): Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. - torques (float, list of float): desired torque(s) to apply to the joint(s) [N]. + joint_ids (int, list[int]): joint id, or list of joint ids. + torques (float, list[float]): desired torque(s) to apply to the joint(s) [N]. """ super(BulletROS, self).set_joint_torques(body_id, joint_ids, torques) if body_id in self.publishers: diff --git a/pyrobolearn/simulators/middlewares/__init__.py b/pyrobolearn/simulators/middlewares/__init__.py index e69de29..91497e6 100644 --- a/pyrobolearn/simulators/middlewares/__init__.py +++ b/pyrobolearn/simulators/middlewares/__init__.py @@ -0,0 +1,14 @@ + +# import middleware abstract class +from .middleware import MiddleWare + +# import ROS +from .ros import ROS + + +# # define decorator +# def middleware(function): +# def wrapper(self, *args, **kwargs): +# if self.middleware is None: +# return function(*args, **kwargs) +# return wrapper diff --git a/pyrobolearn/simulators/middlewares/middleware.py b/pyrobolearn/simulators/middlewares/middleware.py index 9d6cbba..ee1ca47 100644 --- a/pyrobolearn/simulators/middlewares/middleware.py +++ b/pyrobolearn/simulators/middlewares/middleware.py @@ -46,6 +46,10 @@ class MiddleWare(object): self.publish = publish self.teleoperate = teleoperate + ############## + # Properties # + ############## + @property def subscribe(self): return self._subscribe @@ -69,3 +73,178 @@ class MiddleWare(object): @teleoperate.setter def teleoperate(self, teleoperate): self._teleoperate = bool(teleoperate) + + ########### + # Methods # + ########### + + def has_sensor(self, body_id, name): + """ + Check if the specified robot has the given sensor. + + Args: + body_id (int): body unique id. + name (str): name of the sensor. + + Returns: + bool: True if the specified robot has the given sensor. + """ + pass + + def get_sensor_values(self, body_id, name): + """ + Return the sensor. + + Args: + body_id (int): body unique id. + name (str): name of the sensor. + + Returns: + np.array, dict, list, None: sensor values. None if it didn't have anything. + """ + pass + + def get_joint_positions(self, body_id, joint_ids): + """ + Get the position of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint position [rad] + if multiple joints: + np.array[float[N]]: joint positions [rad] + """ + pass + + def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None, + check_teleoperate=False): + """ + Set the position of the given joint(s) (using position control). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + positions (float, np.array[float[N]]): desired position, or list of desired positions [rad] + velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + kps (None, float, np.array[float[N]]): position gain(s) + kds (None, float, np.array[float[N]]): velocity gain(s) + forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values. + check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True, + and if so, it will set the joint positions. If the `teleoperate` argument has been set to False, it + won't set the joint positions. + """ + pass + + def get_joint_velocities(self, body_id, joint_ids): + """ + Get the velocity of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint velocity [rad/s] + if multiple joints: + np.array[float[N]]: joint velocities [rad/s] + """ + pass + + def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None, check_teleoperate=False): + """ + Set the velocity of the given joint(s) (using velocity control). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + max_force (None, float, np.array[float[N]]): maximum motor forces/torques. + check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True, + and if so, it will set the joint velocities. If the `teleoperate` argument has been set to False, it + won't set the joint velocities. + """ + pass + + def get_joint_torques(self, body_id, joint_ids): + """ + Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`. + Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the + applied joint motor torque is exactly what you provide, so there is no need to report it separately." [1] + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: torque [Nm] + if multiple joints: + np.array[float[N]]: torques associated to the given joints [Nm] + """ + pass + + def set_joint_torques(self, body_id, joint_ids, torques, check_teleoperate=False): + """ + Set the torque/force to the given joint(s) (using force/torque control). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + torques (float, list[float]): desired torque(s) to apply to the joint(s) [N]. + check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True, + and if so, it will set the joint torques. If the `teleoperate` argument has been set to False, it won't + set the joint torques. + """ + pass + + def get_jacobian(self, body_id, link_id, local_position=None, q=None): + r""" + Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that: + + .. math:: v = [\dot{p}, \omega]^T = J(q) \dot{q} + + where :math:`\dot{p}` is the Cartesian linear velocity of the link, and :math:`\omega` is its angular velocity. + + Warnings: if we have a floating base then the Jacobian will also include columns corresponding to the root + link DoFs (at the beginning). If it is a fixed base, it will only have columns associated with the joints. + + Args: + body_id (int): unique body id. + link_id (int): link id. + local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link local + coordinates around its center of mass). If None, it will use the CoM position (in the link frame). + q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs. + + Returns: + np.array[float[6,N]], np.array[float[6,6+N]]: full geometric (linear and angular) Jacobian matrix. The + number of columns depends if the base is fixed or floating. + """ + pass + + def get_inertia_matrix(self, body_id, q): + r""" + Return the mass/inertia matrix :math:`H(q)`, which is used in the rigid-body equation of motion (EoM) in joint + space given by (see [1]): + + .. math:: \tau = H(q)\ddot{q} + C(q,\dot{q}) + + where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and + :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any + other forces acting on the system except the applied torques :math:`\tau`. + + Warnings: If the base is floating, it will return a [6+N,6+N] inertia matrix, where N is the number of actuated + joints. If the base is fixed, it will return a [N,N] inertia matrix + + Args: + body_id (int): body unique id. + q (np.array[float[N]]): joint positions of size N, where N is the total number of DoFs. + + Returns: + np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix + """ + pass diff --git a/pyrobolearn/simulators/middlewares/ros.py b/pyrobolearn/simulators/middlewares/ros.py index 0ab1e61..53b5a42 100644 --- a/pyrobolearn/simulators/middlewares/ros.py +++ b/pyrobolearn/simulators/middlewares/ros.py @@ -1,8 +1,35 @@ #!/usr/bin/env python """Define the ROS middleware API. +ROS (Robot Operating System) [1] is a robotics middleware which "provides libraries and tools to help software +developers create robot applications. It provides hardware abstraction, device drivers, libraries, visualizers, +message-passing, package management, and more." + +- rospy [2]: "rospy is a pure Python client library for ROS. The rospy client API enables Python programmers to + quickly interface with ROS Topics, Services, and Parameters." +- ros_control [3]: "A set of packages that include controller interfaces, controller managers, transmissions and + hardware_interfaces." +- robot_state_publisher [4]: "This package allows you to publish the state of a robot to tf. Once the state gets + published, it is available to all components in the system that also use tf. The package takes the joint angles of + the robot as input and publishes the 3D poses of the robot links, using a kinematic tree model of the robot. The + package can both be used as a library and as a ROS node." +- joint_state_publisher [5]: "This package contains a tool for setting and publishing joint state values for a given + URDF." + +Note to compile ROS packages using `catkin_make`, you might have to specify the Python version used using: +- catkin_make -DPYTHON_EXECUTABLE=path/to/bin/python3 + Dependencies in PRL: * `pyrobolearn.simulators.middlewares.middleware.MiddleWare` + +References: + - [1] ROS: http://www.ros.org/ and http://wiki.ros.org + - [2] rospy: http://wiki.ros.org/rospy + - [3] ros_control: http://wiki.ros.org/ros_control + - ROS control an overview: https://roscon.ros.org/2014/wp-content/uploads/2014/07/ros_control_an_overview.pdf + - [4] robot_state_publisher: http://wiki.ros.org/robot_state_publisher + - [5] joint_state_publisher: http://wiki.ros.org/joint_state_publisher + - [6] roslaunch: http://wiki.ros.org/roslaunch/API%20Usage """ # TODO @@ -12,8 +39,21 @@ import psutil import signal import importlib import inspect +import yaml + +import rospy +import rostopic +import controller_manager.controller_manager_interface as cm_interface +import roslaunch +import rosparam +import std_msgs.msg as std_msg +import sensor_msgs.msg as sensor_msg +import gazebo_msgs.msg as gazebo_msg +import geometry_msgs.msg as geometry_msg from pyrobolearn.simulators.middlewares.middleware import MiddleWare +from pyrobolearn.simulators.middlewares.ros_publisher import Publisher, PublisherData, RobotPublisher +from pyrobolearn.simulators.middlewares.ros_subscriber import Subscriber, SubscriberData, RobotSubscriber __author__ = "Brian Delhaisse" @@ -26,6 +66,123 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" +class RobotMiddleWare(object): + r"""Robot middleware interface. + + """ + + def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, control_file=None): + """ + Initialize the robot middleware interface. + + Args: + robot_id (int): robot unique id. + urdf (str): path to the URDF file. + subscribe (bool): if True, it will subscribe to the topics associated to the loaded robot, and will read + the values published on these topics. + publish (bool): if True, it will publish the given values to the topics associated to the loaded robot. + teleoperate (bool): if True, it will move the robot based on the received or sent values based on the 2 + previous attributes :attr:`subscribe` and :attr:`publish`. + control_file (str): path to the YAML control file. + """ + self.id = robot_id + self.urdf = urdf + self.control_file = control_file + self.subscribers = {} + self.publishers = {} + + # set variables + self.subscribe = subscribe + self.publish = publish + self.teleoperate = teleoperate + + def get_joint_positions(self, joint_ids): + """ + Get the position of the given joint(s). + + Args: + joint_ids (int, list[int]): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint position [rad] + if multiple joints: + np.array[float[N]]: joint positions [rad] + """ + q = self.subscribers[body_id].get_joint_positions(joint_ids) + if self.teleoperate and body_id in self.publishers: + self.publishers[body_id].set_joint_positions(joint_ids, q) + self.publishers[body_id].publish('joint_states') + return q + + def set_joint_positions(self, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None): + """ + Set the position of the given joint(s) (using position control). + + Args: + joint_ids (int, list[int]): joint id, or list of joint ids. + positions (float, np.array[float[N]]): desired position, or list of desired positions [rad] + velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + kps (None, float, np.array[float[N]]): position gain(s) + kds (None, float, np.array[float[N]]): velocity gain(s) + forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values. + """ + pass + + def get_joint_velocities(self, joint_ids): + """ + Get the velocity of the given joint(s). + + Args: + joint_ids (int, list[int]): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint velocity [rad/s] + if multiple joints: + np.array[float[N]]: joint velocities [rad/s] + """ + pass + + def set_joint_velocities(self, joint_ids, velocities, max_force=None): + """ + Set the velocity of the given joint(s) (using velocity control). + + Args: + joint_ids (int, list[int]): joint id, or list of joint ids. + velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + max_force (None, float, np.array[float[N]]): maximum motor forces/torques. + """ + pass + + def get_joint_torques(self, joint_ids): + """ + Get the applied torque(s) on the given joint(s). "This is the motor torque applied during the last `step`. + Note that this only applies in VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the + applied joint motor torque is exactly what you provide, so there is no need to report it separately." [1] + + Args: + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: torque [Nm] + if multiple joints: + np.array[float[N]]: torques associated to the given joints [Nm] + """ + pass + + def set_joint_torques(self, joint_ids, torques): + """ + Set the torque/force to the given joint(s) (using force/torque control). + + Args: + joint_ids (int, list[int]): joint id, or list of joint ids. + torques (float, list[float]): desired torque(s) to apply to the joint(s) [N]. + """ + pass + + class ROS(MiddleWare): r"""ROS Interface middleware @@ -60,29 +217,24 @@ class ROS(MiddleWare): self.roscore = subprocess.Popen(["roscore", "-p", str(master_uri)], env=self.env, preexec_fn=os.setsid) # , shell=True) - # set variables - self.subscribe = subscribe - self.publish = publish - self.teleoperate = teleoperate - # remember each publisher/subscriber self.subscribers = {} self.publishers = {} self.models = [] + self._robots = {} # {body_id: RobotMiddleware} + self.count_id = -1 - def close(self): - """ - Close everything - """ - # delete each subscribers + # roslaunch + self.launch = roslaunch.scriptapi.ROSLaunch() + self.launch.start() - # delete each publishers + self.processes = {} # {Node: process} - # delete ROS - if self.roscore is not None: - os.killpg(os.getpgid(self.roscore.pid), signal.SIGTERM) + ############## + # Properties # + ############## @property def is_subscribing(self): @@ -94,6 +246,376 @@ class ROS(MiddleWare): """Return True if we are publishing to topics.""" return self.publish + ########### + # Methods # + ########### + + ############## + # ROS launch # + ############## + + @staticmethod + def create_ros_node(package, node_type, name=None, namespace='/', machine_name=None, args='', respawn=False, + respawn_delay=0.0, remap_args=None, env_args=None, output=None, cwd=None, launch_prefix=None, + required=False, filename=''): + """ + Create a ROS node; data structure for storing information about a desired node in the ROS system Corresponds + to the 'node' tag in the launch specification. + + This basically wraps the `roslaunch.core.Node(...)`. + + Args: + package (str): node package name. + node_type (str): node type. + name (str): node name. + namespace (str): namespace for node. + machine_name (str): name of machine to run node on. + args (str): argument string to pass to node executable. + respawn (bool): if True, respawn node if it dies. + respawn_delay (float): if respawn is True, respawn node after delay. + remap_args (list[tuple[str,str]]): list of [(from, to)] remapping arguments. + env_args (list[tuple[str,str]]): list of [(key, value)] of additional environment vars to set for node. + output (str): where to log output to, either Node, 'screen' or 'log'. + cwd (str): current working directory of node, either 'node', 'ROS_HOME'. Default: ROS_HOME. + launch_prefix (str): launch command/arguments to prepend to node executable arguments. + required (bool): node is required to stay running (launch fails if node dies). + filename (str): name of file Node was parsed from. + + Raises: + ValueError: if parameters do not validate. + + Returns: + roslaunch.core.Node: ROS node data structure. + """ + return roslaunch.core.Node(package=package, node_type=node_type, name=name, namespace=namespace, + machine_name=machine_name, args=args, respawn=respawn, respawn_delay=respawn_delay, + remap_args=remap_args, env_args=env_args, output=output, cwd=cwd, + launch_prefix=launch_prefix, required=required, filename=filename) + + def launch(self, ros_node): + """ + Launch a ROS node. + + Args: + ros_node (roslaunch.core.Node): ROS node. + + Returns: + roslaunch.nodeprocess.LocalProcess: the process. + """ + process = self.launch.launch(ros_node) + self.processes[ros_node] = process + return process + + @staticmethod + def get_namespace(): + """ + Get the namespace. + + Returns: + str: namespace. + """ + return rospy.get_namespace() + + ################################################ + # ROS Topics/Services + Publishers/Subscribers # + ################################################ + + def add_subscriber(self, body_id, topic_name, freq, fct, attribute): + pass + + def add_publisher(self, body_id, topic_name, freq, fct, attribute): + pass + + def remap(self, topic1, topic2, freq): + pass + + def remove_publisher(self, publisher_id): + pass + + def remove_subscriber(self, subscriber_id): + pass + + @staticmethod + def get_topics(namespace='/'): + """ + Get the published topics. + + Returns: + list[list[str, str]]: list of tuples where the first is the topic and the second element is the message + type that topic accepts. + """ + return rospy.get_published_topics(namespace=namespace) + + @staticmethod + def has_topic(name, namespace='/'): + """ + Check if the specified topic has been advertised. + + Warnings: this has a O(N) time complexity. + + Args: + name (str): name of the topic. + namespace (str): namespace. + + Returns: + bool: True if the topic has been advertised. + """ + topics = rospy.get_published_topics(namespace=namespace) + for topic_name, dtype in topics: + if topic_name == name: # or topic_name.split('/')[-1] == name: + return True + return False + + @staticmethod + def get_topic_type(name): + """ + Get the topic type name of the given topic name. + + This is the same as typing the following in the terminal: + - `rostopic type /topic_name` + + Args: + name (str): name of the topic. + + Returns: + str: type of the topic + """ + return rostopic.get_topic_type(name)[0] + + @staticmethod + def get_topic_class(name): + """ + Get the topic class type of the given topic name. + + Args: + name (str): name of the topic. + + Returns: + type: topic class type. This can later be instantiated. + """ + return rostopic.get_topic_class(name) + + ########################### + # ROS parameters (+ YAML) # + ########################### + + @staticmethod + def load_parameter_file(filename, namespace=None): + """ + Load a parameter YAML file. + + Args: + filename (str): path to the YAML file. + namespace (str): default namespace. + + Returns: + list[dict[str,dict[str:dict[str:dict]]], str]: [{robot_name: {param_name: value}}, namespace] + """ + return rosparam.load_file(filename, default_namespace=namespace) + + @staticmethod + def load_parameter_string(parameters, namespace=None): + """ + Load a control configuration YAML string. + + Args: + parameters (str): string in the YAML format specifying the control parameters. + namespace (str): default namespace. + + Returns: + list[dict[str,dict[str:dict[str:dict]]], str]: [{robot_name: {param_name: value}}, namespace] + """ + return rosparam.load_str(parameters, default_namespace=namespace) + + @staticmethod + def set_parameter(name, value): + """ + Set a parameter on the param server. + + Args: + name (str): name of the parameter. + value (str, dict): parameter value. "If param_value is a dictionary it will be treated as a parameter + tree, where param_name is the namespace. For example::: {'x':1,'y':2,'sub':{'z':3}} will set + `name/x=1`, `name/y=2`, and `name/sub/z=3`. Furthermore, it will replace all existing parameters in + the `name` namespace with the parameters in `value`. You must set parameters individually if you wish + to perform a union update. + """ + rospy.set_param(name, value) + + @staticmethod + def get_parameter(name, default=None): + r""" + Get the parameter associated with the given name from the param server. + + Args: + name (str): name of the parameter. + default (object): default value to return if the parameter is not found. + + Returns: + str: parameter value + """ + return rospy.get_param(name, default=default) + + @staticmethod + def get_parameter_names(): + """ + Return the parameter names that have been loaded. + + Returns: + list[str]: list of parameter names. + """ + return rospy.get_param_names() + + @staticmethod + def upload_parameters(values, namespace='/'): + """ + Upload parameters to the Parameter Server. + + Args: + values (dict): dictionary where keys are parameter names and values are parameter values. + namespace (str): namespace to load parameters. + """ + rosparam.upload_params(ns=namespace, values=values) + + @staticmethod + def load_config_file(filename, namespace=None): + """ + Load a control configuration YAML file. + + Args: + filename (str): path to the YAML file. + namespace (str): default namespace. + + Returns: + list[dict[str,dict[str:dict[str:dict]]], str]: [{robot_name: {param_name: value}}, namespace] + """ + params = rosparam.load_file(filename, default_namespace=namespace) + params = params[0] + namespace = params[1] + 'rrbot/' + params = params[0]['rrbot'] + for key, value in params.items(): + rosparam.upload_params(ns=namespace + key, values=value) + + @staticmethod + def load_config_string(parameters, namespace=None): + """ + Load a control configuration YAML string. + + Args: + parameters (str): string in the YAML format specifying the control parameters. + namespace (str): default namespace. + + Returns: + list[list[str,str]]: + """ + params = rosparam.load_str(parameters, default_namespace=namespace) + + ############### + # ROS CONTROL # + ############### + + def get_pid(self, body_id): + pass + + def set_pid(self, body_id, p=None, i=None, d=None): + pass + + @staticmethod + def load_controller(name): + """ + Load controller based on the given name. + + Args: + name (str): name of the controller to load. + + Returns: + bool: True if we could load the controller. + """ + cm_interface.load_controller(name) + + @staticmethod + def unload_controller(name): + """ + Unload the controller based on the given name. + + Args: + name (str): name of the controller to unload. + + Returns: + bool: True if we could unload the controller. + """ + cm_interface.unload_controller(name) + + @staticmethod + def start_controller(name): + """ + Start the controller based on the given name. + + Args: + name (str): name of the controller to start. + + Returns: + bool: True if we could start the controller. + """ + cm_interface.start_controller(name) + + @staticmethod + def stop_controller(name): + """ + Stop the controller based on the given name. + + Args: + name (str): name of the controller to stop. + + Returns: + bool: True if we could stop the controller. + """ + return cm_interface.stop_controller(name) + + @staticmethod + def get_controllers(): + """ + Return the list of controllers. + """ + rospy.wait_for_service('controller_manager/list_controllers') + s = rospy.ServiceProxy('controller_manager/list_controllers', cm_interface.ListControllers) + resp = s.call(cm_interface.ListControllersRequest()) + return [(c.name, list(set(r.hardware_interface for r in c.claimed_resources)), c.state) + for c in resp.controller] + + @staticmethod + def get_controller_types(): + """ + Return the list of controller types. + """ + rospy.wait_for_service('controller_manager/list_controller_types') + s = rospy.ServiceProxy('controller_manager/list_controller_types', cm_interface.ListControllerTypes) + resp = s.call(cm_interface.ListControllerTypesRequest()) + return [t for t in resp.types] + + ############## + # Middleware # + ############## + + def close(self): + """ + Close everything + """ + # delete each subscribers + + # delete each publishers + + for process in self.processes.values(): + process.stop() + + # stop launch + if self.launch: + self.launch.stop() + + # delete ROS + if self.roscore is not None: + os.killpg(os.getpgid(self.roscore.pid), signal.SIGTERM) + def load_urdf(self, filename, position=None, orientation=None, use_maximal_coordinates=None, use_fixed_base=None, flags=None, scale=None): """Load the given URDF file. @@ -106,9 +628,10 @@ class ROS(MiddleWare): Args: filename (str): a relative or absolute path to the URDF file on the file system of the physics server. - position (vec3): create the base of the object at the specified position in world space coordinates [x,y,z] - orientation (quat): create the base of the object at the specified orientation as world space quaternion - [x,y,z,w] + position (np.array[float[3]]): create the base of the object at the specified position in world space + coordinates [x,y,z] + orientation (np.array[float[4]]): create the base of the object at the specified orientation as world + space quaternion [x,y,z,w] use_maximal_coordinates (int): Experimental. By default, the joints in the URDF file are created using the reduced coordinate method: the joints are simulated using the Featherstone Articulated Body algorithm (btMultiBody in Bullet 2.x). The useMaximalCoordinates option will create a 6 degree of freedom rigid @@ -171,6 +694,27 @@ class ROS(MiddleWare): if self.publish: check_ros('publisher', self.publishers, id_) + + # get path to the URDF folder + path = os.path.abspath(filename) # /path/to/pyrobolearn/robots/urdfs//robot.urdf + dirname = str(os.path.dirname(path)) # /path/to/pyrobolearn/robots/urdfs// + basename = str(os.path.basename(path).split('.')[-2]) # robot name without extension + config_file = dirname + basename + ".yaml" + + # check for YAML control configuration file + if os.path.isfile(config_file): + # if it exists, import it + data = yaml.load(open(config_file, 'r'), Loader=yaml.FullLoader) + else: + data = {basename: {'joint_state_controller': {'publish_rate': 50, + 'type': 'joint_state_controller/JointStateController'}}} + + node = roslaunch.core.Node(package="controller_manager", name="controller_spawner", node_type="spawner", + respawn="false", output="screen", namespace="/rrbot", args=' '.join( + ["joint_state_controller", "joint1_position_controller", "joint2_position_controller"])) + + # create subscribers + return id_ def get_joint_positions(self, body_id, joint_ids): @@ -179,33 +723,41 @@ class ROS(MiddleWare): Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. + joint_ids (int, list[int]): joint id, or list of joint ids. Returns: if 1 joint: float: joint position [rad] if multiple joints: - np.float[N]: joint positions [rad] + np.array[float[N]]: joint positions [rad] """ - if body_id in self.subscribers: - return self.subscribers[body_id].get_joint_positions(joint_ids) + robot = self._robots.get(body_id) + if robot is not None: + return robot.get_joint_positions(joint_ids) - def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None): + def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None, + check_teleoperate=False): """ Set the position of the given joint(s) (using position control). Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. - positions (float, np.float[N]): desired position, or list of desired positions [rad] - velocities (None, float, np.float[N]): desired velocity, or list of desired velocities [rad/s] - kps (None, float, np.float[N]): position gain(s) - kds (None, float, np.float[N]): velocity gain(s) - forces (None, float, np.float[N]): maximum motor force(s)/torque(s) used to reach the target values. + joint_ids (int, list[int]): joint id, or list of joint ids. + positions (float, np.array[float[N]]): desired position, or list of desired positions [rad] + velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + kps (None, float, np.array[float[N]]): position gain(s) + kds (None, float, np.array[float[N]]): velocity gain(s) + forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values. + check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True, + and if so, it will set the joint positions. If the `teleoperate` argument has been set to False, it + won't set the joint positions. """ - if body_id in self.publishers: - self.publishers[body_id].set_joint_positions(joint_ids, positions) - self.publishers[body_id].publish('joint_states') + robot = self._robots.get(body_id) + if robot is not None: + if check_teleoperate and not self.teleoperate: + return None + return robot.set_joint_positions(joint_ids, positions, velocities=velocities, kps=kps, kds=kds, + forces=forces) def get_joint_velocities(self, body_id, joint_ids): """ @@ -213,30 +765,36 @@ class ROS(MiddleWare): Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. + joint_ids (int, list[int]): joint id, or list of joint ids. Returns: if 1 joint: float: joint velocity [rad/s] if multiple joints: - np.float[N]: joint velocities [rad/s] + np.array[float[N]]: joint velocities [rad/s] """ - if body_id in self.subscribers: - return self.subscribers[body_id].get_joint_velocities(joint_ids) + robot = self._robots.get(body_id) + if robot is not None: + return robot.get_joint_velocities(joint_ids) - def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None): + def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None, check_teleoperate=False): """ Set the velocity of the given joint(s) (using velocity control). Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. - velocities (float, np.float[N]): desired velocity, or list of desired velocities [rad/s] - max_force (None, float, np.float[N]): maximum motor forces/torques + joint_ids (int, list[int]): joint id, or list of joint ids. + velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + max_force (None, float, np.array[float[N]]): maximum motor forces/torques. + check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True, + and if so, it will set the joint velocities. If the `teleoperate` argument has been set to False, it + won't set the joint velocities. """ - if body_id in self.publishers: - self.publishers[body_id].set_joint_velocities(joint_ids, velocities) - self.publishers[body_id].publish('joint_states') + robot = self._robots.get(body_id) + if robot is not None: + if check_teleoperate and not self.teleoperate: + return None + return robot.set_joint_velocities(joint_ids, velocities, max_force=max_force) def get_joint_torques(self, body_id, joint_ids): """ @@ -246,26 +804,128 @@ class ROS(MiddleWare): Args: body_id (int): unique body id. - joint_ids (int, list of int): a joint id, or list of joint ids. + joint_ids (int, list[int]): a joint id, or list of joint ids. Returns: if 1 joint: float: torque [Nm] if multiple joints: - np.float[N]: torques associated to the given joints [Nm] + np.array[float[N]]: torques associated to the given joints [Nm] """ - if body_id in self.subscribers: - return self.subscribers[body_id].get_joint_torques(joint_ids) + robot = self._robots.get(body_id) + if robot is not None: + return robot.get_joint_torques(joint_ids) - def set_joint_torques(self, body_id, joint_ids, torques): + def set_joint_torques(self, body_id, joint_ids, torques, check_teleoperate=False): """ Set the torque/force to the given joint(s) (using force/torque control). Args: body_id (int): unique body id. - joint_ids (int, list of int): joint id, or list of joint ids. - torques (float, list of float): desired torque(s) to apply to the joint(s) [N]. + joint_ids (int, list[int]): joint id, or list of joint ids. + torques (float, list[float]): desired torque(s) to apply to the joint(s) [N]. + check_teleoperate (bool): if True, it will check if the given `teleoperate` argument has been set to True, + and if so, it will set the joint torques. If the `teleoperate` argument has been set to False, it won't + set the joint torques. """ - if body_id in self.publishers: - self.publishers[body_id].set_joint_torques(joint_ids, torques) - self.publishers[body_id].publish('joint_states') + robot = self._robots.get(body_id) + if robot is not None: + if check_teleoperate and not self.teleoperate: + return None + return robot.set_joint_torques(joint_ids, torques) + + def get_jacobian(self, body_id, link_id, local_position=None, q=None): + r""" + Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that: + + .. math:: v = [\dot{p}, \omega]^T = J(q) \dot{q} + + where :math:`\dot{p}` is the Cartesian linear velocity of the link, and :math:`\omega` is its angular velocity. + + Warnings: if we have a floating base then the Jacobian will also include columns corresponding to the root + link DoFs (at the beginning). If it is a fixed base, it will only have columns associated with the joints. + + Args: + body_id (int): unique body id. + link_id (int): link id. + local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link local + coordinates around its center of mass). If None, it will use the CoM position (in the link frame). + q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs. + + Returns: + np.array[float[6,N]], np.array[float[6,6+N]]: full geometric (linear and angular) Jacobian matrix. The + number of columns depends if the base is fixed or floating. + """ + pass + + def get_inertia_matrix(self, body_id, q): + r""" + Return the mass/inertia matrix :math:`H(q)`, which is used in the rigid-body equation of motion (EoM) in joint + space given by (see [1]): + + .. math:: \tau = H(q)\ddot{q} + C(q,\dot{q}) + + where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and + :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any + other forces acting on the system except the applied torques :math:`\tau`. + + Warnings: If the base is floating, it will return a [6+N,6+N] inertia matrix, where N is the number of actuated + joints. If the base is fixed, it will return a [N,N] inertia matrix + + Args: + body_id (int): body unique id. + q (np.array[float[N]]): joint positions of size N, where N is the total number of DoFs. + + Returns: + np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix + """ + pass + + def has_sensor(self, body_id, name): + """ + Check if the specified robot has the given sensor. + + Args: + body_id (int): body unique id. + name (str): name of the sensor. + + Returns: + bool: True if the specified robot has the given sensor. + """ + robot = self._robots.get(body_id) + if robot is not None: + return robot.has_sensor(name) + return False + + def get_sensor_values(self, body_id, name): + """ + Return the sensor. + + Args: + body_id (int): body unique id. + name (str): name of the sensor. + + Returns: + np.array, dict, list, None: sensor values. None if it didn't have anything. + """ + robot = self._robots.get(body_id) + if robot is not None: + return robot.get_sensor_values(name) + + def can_teleoperate(self, body_id): + robot = self._robots.get(body_id, None) + if robot is None: + return False + return robot.teleoperate + + def can_subscribe(self, body_id): + robot = self._robots.get(body_id, None) + if robot is None: + return False + return robot.subscribe + + def can_publish(self, body_id): + robot = self._robots.get(body_id, None) + if robot is None: + return False + return robot.publish diff --git a/pyrobolearn/simulators/middlewares/ros_publisher.py b/pyrobolearn/simulators/middlewares/ros_publisher.py new file mode 100644 index 0000000..264ad7a --- /dev/null +++ b/pyrobolearn/simulators/middlewares/ros_publisher.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python +"""Define the abstract robot publisher. +""" + +import rospy + +# import the messages +from std_msgs import msg as std_msg +from sensor_msgs import msg as sensor_msg +from geometry_msgs import msg as geometry_msg + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class PublisherData(object): + r"""Publisher data holder + """ + + def __init__(self, topic, data_class, queue_size=10): + self.__dict__['publisher'] = rospy.Publisher(topic, data_class, queue_size=queue_size) + self.__dict__['attributes'] = [attr for attr in [attr for attr in dir(data_class) if not attr.startswith('_')] + if not callable(getattr(data_class, attr))] + self.__dict__['publisher_data'] = data_class() + + def publish(self, data=None): + if data is None: + self.publisher.publish(self.publisher_data) + else: + self.publisher.publish(data) + + def __setattr__(self, key, value): + if key in self.attributes: + setattr(self.publisher_data, key, value) + + def __getattr__(self, key): + return getattr(self.publisher_data, key) + + +class Publisher(object): + r"""Publisher class + + This Publisher abstract class is the class from which all the other publishers inherit from. It provides the + common functionalities between the various publishers. + """ + + def __init__(self, publisher_id=None): + """ + Initialize the publisher. + + Args: + publisher_id (int, None): publisher id which is used when initializing the node. If None, a name will be + auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous` + parameter in `rospy.init_node`. + """ + + # initialize the node + if publisher_id is None: + rospy.init_node(self.__class__.__name__, anonymous=True) + else: + rospy.init_node(self.__class__.__name__ + str(publisher_id)) + + # all publishers + self.publishers = dict() + + def create_publisher(self, name, topic, data_class): + """ + Create a publisher to the specific topic. + + Args: + name (str): unique name of the publisher. The name must be unique. You will be able to access to this + topic (str): name of the topic. + data_class (object): data type class to use for messages + + Returns: + PublisherData: the publisher data holder. + """ + publisher = PublisherData(topic, data_class) + self.publishers[name] = publisher + setattr(self, name, publisher) + return publisher + + def publish(self, name=None, data=None): + if name is None and data is None: + for publisher in self.publishers.values(): + publisher.publish() + elif name is not None: + self.__dict__[name].publish(data) + + # def __getattr__(self, name): + # return self.publishers[name] + + +class RobotPublisher(Publisher): + r"""Robot Publisher class + + This Robot Publisher class is the class from which all the robot publishers inherit from. + """ + + def __init__(self, name, id_=None): + r""" + Initialize the robot publisher. + + Args: + name (str): name of the robot. This will be used to create the topics. + id_ (int, None): robot id which is used when initializing the node. If None, a name will be + auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous` + parameter in `rospy.init_node`. + """ + super(RobotPublisher, self).__init__(publisher_id=id_) + self.name = name.lower() + + # create Joint states + # self.create_publisher('joint_states', self.name + '/joint_states', sensor_msg.JointState) + self.joint_states = PublisherData(self.name + '/joint_states', sensor_msg.JointState) + self.publishers['joint_states'] = self.joint_states + + def set_joint_positions(self, joint_ids, positions): + # self.joint_states.position[joint_ids] = positions + self.joint_states.position = positions + + def set_joint_velocities(self, joint_ids, velocities): + self.joint_states.velocity[joint_ids] = velocities + + def set_joint_torques(self, joint_ids, torques): + self.joint_states.effort[joint_ids] = torques + + +# Tests +if __name__ == '__main__': + # NOTE: run roscore before hand + import numpy as np + from itertools import count + import time + + publisher = RobotPublisher('walter') + print("Published topics: {}".format(rospy.get_published_topics())) + print("Robot joint state attributes: {}".format(publisher.joint_states.attributes)) + + publisher.joint_states.position = np.array(range(3)) + + for t in count(): + print(t) + publisher.publish() + time.sleep(0.1) diff --git a/pyrobolearn/simulators/middlewares/ros_subscriber.py b/pyrobolearn/simulators/middlewares/ros_subscriber.py new file mode 100644 index 0000000..a4b01c2 --- /dev/null +++ b/pyrobolearn/simulators/middlewares/ros_subscriber.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python +"""Define the abstract robot subscriber. +""" + +import numpy as np +import rospy + +# import the messages +# from std_msgs import msg as std_msg +from sensor_msgs import msg as sensor_msg +# from geometry_msgs import msg as geometry_msg + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class SubscriberData(object): + r"""Subscriber data holder + """ + + def __init__(self, topic, data_class): + self.subscriber = rospy.Subscriber(topic, data_class, callback=self.callback) + self.attributes = set([attr for attr in [attr for attr in dir(data_class) if not attr.startswith('_')] + if not callable(getattr(data_class, attr))]) + self.subscriber_data = data_class() + + def callback(self, data): + self.subscriber_data = data + + def __getattr__(self, name): + return getattr(self.subscriber_data, name) + + def unregister(self): + self.subscriber.unregister() + + +class Subscriber(object): + r"""Subscriber class + + This Subscriber abstract class is the class from which all the other subscribers inherit from. It provides the + common functionalities between the various subscribers. + """ + + def __init__(self, subscriber_id=None): + """ + Initialize the subscriber. + + Args: + subscriber_id (int, None): subscriber id which is used when initializing the node. If None, a name will be + auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous` + parameter in `rospy.init_node`. + """ + + # initialize the node + if subscriber_id is None: + rospy.init_node(self.__class__.__name__, anonymous=True) + else: + rospy.init_node(self.__class__.__name__ + str(subscriber_id)) + + # all subscribers + self.subscribers = dict() + + def create_subscriber(self, name, topic, data_class): + """ + Create a subscriber to the specific topic. + + Args: + name (str): unique name of the subscriber. The name must be unique. You will be able to access to this + topic (str): name of the topic. + data_class (object): data type class to use for messages + + Returns: + SubscriberData: the subscriber data holder. + """ + subscriber = SubscriberData(topic, data_class) + self.subscribers[name] = subscriber + return subscriber + + def __getattr__(self, name): + return self.subscribers[name] + + def unregister(self, name=None): + if name is None: + for subscriber in self.subscribers.values(): + subscriber.unregister() + else: + self.subscribers[name].unregister() + + def close(self): + self.unregister() + + def __del__(self): + self.unregister() + + +class RobotSubscriber(Subscriber): + r"""Robot Subscriber class + + This Robot Subscriber class is the class from which all the robot subscribers inherit from. + """ + + def __init__(self, name, id_=None): + r""" + Initialize the robot subscriber. + + Args: + name (str): name of the robot. This will be used to create the topics. + id_ (int, None): robot id which is used when initializing the node. If None, a name will be + auto-generated for the name using name as the base. See the documentation for the :attr:`anonymous` + parameter in `rospy.init_node`. + """ + super(RobotSubscriber, self).__init__(subscriber_id=id_) + self.name = name.lower() + + # create Joint states + self.create_subscriber('joint_states', self.name + '/joint_states', sensor_msg.JointState) + + def get_joint_positions(self, joint_ids): + if len(self.joint_states.position) >= len(joint_ids): + return np.asarray(self.joint_states.position) # [joint_ids] + return np.asarray(self.joint_states.position) + + def get_joint_velocities(self, joint_ids): + if len(self.joint_states.velocity) >= len(joint_ids): + return np.asarray(self.joint_states.velocity)[joint_ids] + return np.asarray(self.joint_states.velocity) + + def get_joint_torques(self, joint_ids): + if len(self.joint_states.effort) >= len(joint_ids): + return np.asarray(self.joint_states.effort)[joint_ids] + return np.asarray(self.joint_states.effort) + + +# Tests +if __name__ == '__main__': + # NOTE: run roscore before hand and don't forget to run the publisher code + import time + from itertools import count + + subscriber = RobotSubscriber('walter') + print("Published topics: {}".format(rospy.get_published_topics())) + print("Robot joint state attributes: {}".format(subscriber.joint_states.attributes)) + + for t in count(): + print(t) + print("Joint position data: {}".format(subscriber.joint_states.position)) + time.sleep(0.1) diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py index 8916115..f49539e 100644 --- a/pyrobolearn/simulators/mujoco.py +++ b/pyrobolearn/simulators/mujoco.py @@ -69,8 +69,10 @@ except ImportError as e: # import glfw # GLFW: OpenGL library for creating windows, contexts and surfaces, receiving input and events. This is used in # mujoco_py -import glfw - +try: + import glfw +except ImportError as e: + raise ImportError(str(e) + "\nTry to install GLFW: `pip install glfw`") # import pyrobolearn related functionalities from pyrobolearn.simulators.simulator import Simulator @@ -155,8 +157,8 @@ class Body(object): # keep in memory the body # self.body = body - self.joints = list(body.joints.values()) - self.links = list(body.bodies.values()) + self.joints = np.array(body.joints.values()) + self.links = np.array(body.bodies.values()) # compute mapping from joint ids to q indices idx, jnt_to_q = 0, [] @@ -333,7 +335,7 @@ class Body(object): if isinstance(q, float): if q != -1: return q - return [] + return None return q[q != -1] # remove fixed joints def get_dq_idx(self, joint_id, keep=False): @@ -369,6 +371,22 @@ class Body(object): return transform_inertial_frame_to_child_inertial_frame(body) +class StateIndices(object): + + def __init__(self): + self.qpos = None + self.qvel = None + self.act = None + self.mocap_pos = None + self.mocap_quat = None + self.userdata = None + self.qacc_warmstart = None + + def reset(self): + self.qpos, self.qvel, self.act, self.mocap_pos, self.mocap_quat = None, None, None, None, None + self.userdata, self.qacc_warmstart = None, None + + class Mujoco(Simulator): r"""Mujoco Simulator interface. @@ -461,10 +479,17 @@ class Mujoco(Simulator): self._root = self._parser.root self._worldbody = self._parser.worldbody + self._state_indices = StateIndices() + # add light self._parser.add_element("light", self._worldbody, attributes={"diffuse": "0.5 0.5 0.5", "pos": "0 0 3", "directional": "true", "dir": "0 0 -1"}) + + # add world camera + self._parser.add_element("camera", self._worldbody, attributes={"name": "prl_world_camera", + "fovy": "45", "pos": "0 0 0"}) + # add floor self.load_floor() @@ -545,6 +570,7 @@ class Mujoco(Simulator): render (bool): if we should render or not. """ # self.render(enable=False) # to delete the previous viewer instance if defined + state = None if self.sim is None else self._save_state() # create the model # self.model = mujoco.load_model_from_path(path) @@ -554,13 +580,17 @@ class Mujoco(Simulator): # create the simulator from the model self.sim = mujoco.MjSim(self.model) + # load the state + if state is not None: + self._load_state(state, self._state_indices) + self._state_indices.reset() + # if we need to render if render: # self.render(enable=True) # to instantiate the viewer if self.viewer is None: self.render(enable=True) else: - print("Update the viewer's sim") self.viewer.update_sim(self.sim) @staticmethod @@ -620,6 +650,79 @@ class Mujoco(Simulator): return '_'.join(name.split('_')[1:-1]) return name + def _save_state(self): + # check: http://www.mujoco.org/book/programming.html#siStateControl + + # copy simulation state + t = self.sim.data.time + qpos = self.sim.data.qpos + qvel = self.sim.data.qvel + act = self.sim.data.act + + # copy mocap body pose and user data + mocap_pos = self.sim.data.mocap_pos + mocap_quat = self.sim.data.mocap_quat + userdata = self.sim.data.userdata + + # copy warm-start acceleration + qacc_warmstart = self.sim.data.qacc_warmstart + + return t, qpos, qvel, act, mocap_pos, mocap_quat, userdata, qacc_warmstart + + def _clear_control(self): + self.sim.data.ctrl[:] = 0 + self.sim.data.qfrc_applied[:] = 0 + self.sim.data.xfrc_applied[:, :] = 0 + + def _load_state(self, state, indices=None): + t, qpos, qvel, act, mocap_pos, mocap_quat, userdata, qacc_warmstart = state + if indices is None: + indices = self._state_indices + + self.sim.data.time = t + + if qpos is not None: + if indices.qpos is None: + self.sim.data.qpos[:len(qpos)] = qpos + else: + self.sim.data.qpos[indices.qpos] = qpos + + if qvel is not None: + if indices.qvel is None: + self.sim.data.qvel[:len(qvel)] = qvel + else: + self.sim.data.qvel[indices.qvel] = qvel + + if act is not None: + if indices.act is None: + self.sim.data.act[:len(act)] = act + else: + self.sim.data.act[indices.act] = act + + if mocap_pos is not None: + if indices.mocap_pos is None: + self.sim.data.mocap_pos[:len(mocap_pos)] = mocap_pos + else: + self.sim.data.mocap_pos[indices.mocap_pos] = mocap_pos + + if mocap_quat is not None: + if indices.mocap_quat is None: + self.sim.data.mocap_quat[:len(mocap_quat)] = mocap_quat + else: + self.sim.data.mocap_quat[indices.mocap_quat] = mocap_quat + + if userdata is not None: + if indices.userdata is None: + self.sim.data.userdata[:len(userdata)] = userdata + else: + self.sim.data.userdata[indices.userdata] = userdata + + if qacc_warmstart is not None: + if indices.qacc_warmstart is None: + self.sim.data.qacc_warmstart[:len(qacc_warmstart)] = qacc_warmstart + else: + self.sim.data.qacc_warmstart[indices.qacc_warmstart] = qacc_warmstart + ################# # utils methods # ################# @@ -680,6 +783,22 @@ class Mujoco(Simulator): self.viewer = mujoco.MjViewer(self.sim) self.viewer.render() + # select with the mouse + coordinates = np.zeros(3) + geomid, skin = 0, 0 + + # mouse selection. + mujoco.functions.mjv_select(self.model, self.sim.data, self.viewer.vopt, aspectratio, relx, rely, + self.viewer.scn, coordinates, geomid, skin) + + # Move perturb object with mouse; action is mjtMouse. + action = 0 + mujoco.functions.mjv_movePerturb(self.model, self.sim.data, action, reldx, reldy, self.viewer.scn, + self.viewer.pert) + + # Set perturb force,torque in d->xfrc_applied, if selected body is dynamic. + mujoco.functions.mjv_applyPerturbForce(self.model, self.sim.data, self.viewer.pert) + # sleep the specified amount of time # time.sleep(sleep_time) @@ -737,6 +856,22 @@ class Mujoco(Simulator): self._parser.option_tag.attrib.setdefault('timestep', time_step) self._update_sim() + def pause(self): + """Pause the simulator if in real-time.""" + pass + + def unpause(self): + """Unpause the simulator if in real-time.""" + pass + + def get_physics_properties(self): + """Get the physics engine parameters.""" + pass + + def set_physics_properties(self, *args, **kwargs): + """Set the physics engine parameters.""" + pass + def get_gravity(self): """Return the gravity set in the simulator.""" return self.sim.model.opt.gravity @@ -1769,7 +1904,7 @@ class Mujoco(Simulator): # compute reaction force force_parent = self.sim.data.cfrc_int[body.b_idx0 + joint_id] # com-based interaction force with parent force_ext = self.sim.data.cfrc_ext[body.b_idx0 + joint_id] # com-based external force on body - force = force_ext - force_parent # TODO: is it + instead of -? + force = force_ext + force_parent # TODO: is it - instead of +? np.roll(force, shift=3, axis=force.ndim - 1) # [torque, force] --> [force, torque] # TODO: express it in the joint frame @@ -1810,14 +1945,37 @@ class Mujoco(Simulator): velocity (float): the joint velocity (angular [rad/s] or linear velocity [m/s]) """ body = self._bodies[body_id] - if not isinstance(joint_id, int): - raise TypeError("Expecting the given joint id to be an int, but got instead: {}".format(type(joint_id))) - if joint_id < 0 or joint_id > (body.num_joints - 1): - raise ValueError("joint_id should belong to [0, `num_joints-1`].") + joint_id = self._check_joint_id(body, joint_id) q = body.get_q_idx(joint_id, keep=True) if q != -1: - self.sim.data.qpos[body.q_idx1 + q] = position - self.sim.data.qvel[body.v_idx1 + q] = velocity + self.model.qpos0[body.q_idx1 + q] = position + # self.sim.data.qpos[body.q_idx1 + q] = position + if velocity is not None: + self.sim.data.qvel[body.v_idx1 + q] = velocity + + def reset_joint_states(self, body_id, joint_ids=None, positions=None, velocities=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. + + Args: + body_id (int): unique body id. + joint_ids (list[int]): joint index in range [0..num_joints(body_id)] + positions (np.array[float]): the joint positions (angle in radians [rad] or position [m]) + velocities (np.array[float]): the joint velocities (angular [rad/s] or linear velocity [m/s]) + """ + body = self._bodies[body_id] + if joint_ids is None: + self.model.qpos0[body.q_idx1:body.q_idxf] = positions + else: + joint_ids = self._check_joint_ids(body, joint_ids) + q = body.get_q_idx(joint_ids, keep=False) + if q is None: + return + self.model.qpos0[body.q_idx1 + q] = positions + # self.sim.data.qpos[body.q_idx1 + q] = positions + if velocities is not None: + self.sim.data.qvel[body.v_idx1 + q] = velocities def enable_joint_force_torque_sensor(self, body_id, joint_ids, enable=True): """ @@ -1850,21 +2008,26 @@ class Mujoco(Simulator): body_id (int): body unique id. joint_ids (int): joint/link id, or list of joint ids. control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD), - VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). + VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). positions (float, np.array[float[N]]): target joint position(s) (used in POSITION_CONTROL). velocities (float, np.array[float[N]]): target joint velocity(ies). In VELOCITY_CONTROL and - POSITION_CONTROL, the target velocity(ies) is(are) the desired velocity of the joint. Note that the - target velocity(ies) is(are) not the maximum joint velocity(ies). In PD_CONTROL and - POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocities are computed using: - `kp*(erp*(desiredPosition-currentPosition)/dt)+currentVelocity+kd*(m_desiredVelocity - currentVelocity)` + POSITION_CONTROL, the target velocity(ies) is(are) the desired velocity of the joint. Note that the + target velocity(ies) is(are) not the maximum joint velocity(ies). In PD_CONTROL and + POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocities are computed using: + `kp*(erp*(desiredPosition-currentPosition)/dt)+currentVelocity+kd*(m_desiredVelocity - currentVelocity)` forces (float, list[float]): in POSITION_CONTROL and VELOCITY_CONTROL, these are the maximum motor - forces used to reach the target values. In TORQUE_CONTROL these are the forces / torques to be applied - each simulation step. + forces used to reach the target values. In TORQUE_CONTROL these are the forces / torques to be applied + each simulation step. kp (float, list[float]): position (stiffness) gain(s) (used in POSITION_CONTROL). kd (float, list[float]): velocity (damping) gain(s) (used in POSITION_CONTROL). max_velocity (float): in POSITION_CONTROL this limits the velocity to a maximum. """ - pass + if control_mode == Simulator.POSITION_CONTROL: + self.set_joint_positions(body_id, joint_ids, positions, velocities, kp, kd, forces) + elif control_mode == Simulator.VELOCITY_CONTROL: + self.set_joint_velocities(body_id, joint_ids, velocities, forces) + elif control_mode == Simulator.TORQUE_CONTROL: + self.set_joint_torques(body_id, joint_ids, forces) def get_link_state(self, body_id, link_id, compute_velocity=False, compute_forward_kinematics=False): """ @@ -2033,7 +2196,7 @@ class Mujoco(Simulator): if multiple links: np.array[float[N,3]]: CoM position of each link in world space """ - return self._get_link_result(body_id, link_ids, self.sim.data.xipos) + return self._get_link_result(body_id, link_ids, self.sim.data.body_xpos) # TODO: xipos vs body_xpos def get_link_positions(self, body_id, link_ids): pass @@ -2325,7 +2488,21 @@ class Mujoco(Simulator): if multiple joints: np.array[float[N]]: maximum force for each specified joint [N] """ - pass + body = self._bodies[body_id] + joint = body.get_joint(joint_ids) + one_joint = isinstance(joint, struct.Joint) + if one_joint: + joint = [joint] + forces = [] + for j in joint: + force = j.effort + if force is None: + forces.append(0.) + else: + forces.append(force) + if one_joint: + return forces[0] + return forces def get_joint_max_velocities(self, body_id, joint_ids): """ @@ -2343,7 +2520,21 @@ class Mujoco(Simulator): if multiple joints: np.array[float[N]]: maximum velocities for each specified joint [rad/s] """ - pass + body = self._bodies[body_id] + joint = body.get_joint(joint_ids) + one_joint = isinstance(joint, struct.Joint) + if one_joint: + joint = [joint] + velocities = [] + for j in joint: + vel = j.velocity + if vel is None: + velocities.append(0.) + else: + velocities.append(vel) + if one_joint: + return velocities[0] + return velocities def get_joint_axes(self, body_id, joint_ids): """ @@ -2359,7 +2550,15 @@ class Mujoco(Simulator): if multiple joint: np.array[float[N,3]]: list of joint axis """ - pass + body = self._bodies[body_id] + q = body.get_q_idx(joint_ids, keep=True) # -1 for fixed joint + if isinstance(q, float): + if q == -1: # fixed joint + return np.zeros(3) + return self.model.jnt_axis[body.j_idx0 + q] + axes = np.zeros((len(q), 3)) + axes[q != -1] = self.model.jnt_axis[body.j_idx0 + q[q != -1]] + return axes def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None): """ @@ -2378,19 +2577,39 @@ class Mujoco(Simulator): body = self._bodies[body_id] + q = self.get_joint_positions(body_id, joint_ids=joint_ids) + qvel= self.get_joint_velocities(body_id, joint_ids=joint_ids) + + if kps is None: + kps = 1000. + if kds is None: + kds = 1. + if velocities is None: + velocities = 0. + + tau = kps * (positions - q) + kds * (velocities - qvel) + if joint_ids is None: - self.sim.data.qpos[body.q_idx1:body.q_idxf] = positions + # self.sim.data.qpos[body.q_idx1:body.q_idxf] = positions + c_q_dq = self.sim.data.qfrc_bias[body.v_idx1:body.v_idxf] + self.sim.data.qfrc_applied[body.v_idx1:body.v_idxf] = tau + c_q_dq - # check if valid joints - self._check_joint_ids(body, joint_ids) + else: + # check if valid joints + self._check_joint_ids(body, joint_ids) - # if one joint, set its torque - if isinstance(joint_ids, int): - self.sim.data.qpos[body.q_idx1 + joint_ids] = positions + # if one joint, set its torque + if isinstance(joint_ids, int): + # self.sim.data.qpos[body.q_idx1 + joint_ids] = positions + c_q_dq = self.sim.data.qfrc_bias[body.v_idx1 + joint_ids] + self.sim.data.qfrc_applied[body.v_idx1 + joint_ids] = tau + c_q_dq - # if multiple joints, set their torques - q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints) - self.sim.data.qpos[body.q_idx1 + q[q != -1]] = positions + # if multiple joints, set their torques + q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints) + + # self.sim.data.qpos[body.q_idx1 + q[q != -1]] = positions + c_q_dq = self.sim.data.qfrc_bias[body.v_idx1 + q[q != -1]] + self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = tau + c_q_dq def get_joint_positions(self, body_id, joint_ids=None): """ @@ -2441,17 +2660,17 @@ class Mujoco(Simulator): if joint_ids is None: self.sim.data.qvel[body.v_idx1:body.v_idxf] = velocities + else: + # check if valid joints + self._check_joint_ids(body, joint_ids) - # check if valid joints - self._check_joint_ids(body, joint_ids) + # if one joint, set its torque + if isinstance(joint_ids, int): + self.sim.data.qvel[body.v_idx1 + joint_ids] = velocities - # if one joint, set its torque - if isinstance(joint_ids, int): - self.sim.data.qvel[body.v_idx1 + joint_ids] = velocities - - # if multiple joints, set their torques - q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints) - self.sim.data.qvel[body.v_idx1 + q[q != -1]] = velocities + # if multiple joints, set their torques + q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints) + self.sim.data.qvel[body.v_idx1 + q[q != -1]] = velocities def get_joint_velocities(self, body_id, joint_ids=None): """ @@ -2543,17 +2762,17 @@ class Mujoco(Simulator): if joint_ids is None: self.sim.data.qfrc_applied[body.v_idx1:body.v_idxf] = torques + else: + # check if valid joints + self._check_joint_ids(body, joint_ids) - # check if valid joints - self._check_joint_ids(body, joint_ids) + # if one joint, set its torque + if isinstance(joint_ids, int): + self.sim.data.qfrc_applied[body.v_idx1 + joint_ids] = torques - # if one joint, set its torque - if isinstance(joint_ids, int): - self.sim.data.qfrc_applied[body.v_idx1 + joint_ids] = torques - - # if multiple joints, set their torques - q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints) - self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = torques + # if multiple joints, set their torques + q = body.get_q_idx(joint_ids, keep=True) # E.g. [0, -1, 1, -1, 2, 3] (-1 are for fixed joints) + self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = torques def get_joint_torques(self, body_id, joint_ids=None): """ @@ -2621,7 +2840,7 @@ class Mujoco(Simulator): # com-based external force on body [torque, force] force_ext = self.sim.data.cfrc_ext[body.b_idx0 + joint_ids] - force = force_ext - force_parent # TODO: is it + instead of -? + force = force_ext + force_parent # TODO: is it - instead of +? np.roll(force, shift=3, axis=force.ndim-1) # [torque, force] --> [force, torque] @@ -2794,6 +3013,82 @@ class Mujoco(Simulator): # return texture id return self._texture_cnt - 1 + def compute_view_matrix(self, eye_position, target_position, up_vector): + """Compute the view matrix. + + The view matrix is the 4x4 matrix that maps the world coordinates into the camera coordinates. Basically, + it applies a rotation and translation such that the world is in front of the camera. That is, instead + of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world. + + Args: + eye_position (np.array[float[3]]): eye position in Cartesian world coordinates + target_position (np.array[float[3]]): position of the target (focus) point in Cartesian world coordinates + up_vector (np.array[float[3]]): up vector of the camera in Cartesian world coordinates + + Returns: + np.array[float[4,4]]: the view matrix + """ + pass + + def compute_view_matrix_from_ypr(self, target_position, distance, yaw, pitch, roll, up_axis_index=2): + """Compute the view matrix from the yaw, pitch, and roll angles. + + The view matrix is the 4x4 matrix that maps the world coordinates into the camera coordinates. Basically, + it applies a rotation and translation such that the world is in front of the camera. That is, instead + of turning the camera to capture what we want in the world, we keep the camera fixed and turn the world. + + Args: + target_position (np.array[float[3]]): target focus point in Cartesian world coordinates + distance (float): distance from eye to focus point + yaw (float): yaw angle in radians left/right around up-axis + pitch (float): pitch in radians up/down. + roll (float): roll in radians around forward vector + up_axis_index (int): either 1 for Y or 2 for Z axis up. + + Returns: + np.array[float[4,4]]: the view matrix + """ + pass + + def compute_projection_matrix(self, left, right, bottom, top, near, far): + """Compute the orthographic projection matrix. + + The projection matrix is the 4x4 matrix that maps from the camera/eye coordinates to clipped coordinates. + It is applied after the view matrix. + + There are 2 projection matrices: + * orthographic projection + * perspective projection + + For the perspective projection, see `computeProjectionMatrixFOV(self)`. + + Args: + left (float): left screen (canvas) coordinate + right (float): right screen (canvas) coordinate + bottom (float): bottom screen (canvas) coordinate + top (float): top screen (canvas) coordinate + near (float): near plane distance + far (float): far plane distance + + Returns: + np.array[float[4,4]]: the perspective projection matrix + """ + pass + + def compute_projection_matrix_fov(self, fov, aspect, near, far): + """Compute the perspective projection matrix using the field of view (FOV). + + Args: + fov (float): field of view + aspect (float): aspect ratio + near (float): near plane distance + far (float): far plane distance + + Returns: + np.array[float[4,4]]: the perspective projection matrix + """ + pass + # TODO: change such that we don't return the width and height (the user already knows them) # TODO: check for segmentation image def get_camera_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None, @@ -2826,10 +3121,16 @@ class Mujoco(Simulator): np.array[float[width, height]]: Depth buffer. np.array[int[width, height]]: Segmentation mask buffer. For each pixels the visible object unique id. """ - # based on the arguments, check the camera name - camera_name = None + camera_name = "prl_world_camera" - return width, height, self.sim.render(width, height, camera_name, depth=True), None + # based on camera + + # mjvCamera + + rgb, depth = self.sim.render(width, height, camera_name, depth=True) + segmentation = -np.ones(width, height) + rgba = np.dstack((rgb, -segmentation)) + return width, height, rgba, depth, segmentation def get_rgba_image(self, width, height, view_matrix=None, projection_matrix=None, light_direction=None, light_color=None, light_distance=None, shadow=None, light_ambient_coeff=None, @@ -2980,6 +3281,121 @@ class Mujoco(Simulator): """ pass + def get_overlapping_objects(self, aabb_min, aabb_max): + """ + This query will return all the unique ids of objects that have Axis Aligned Bounding Box (AABB) overlap with + a given axis aligned bounding box. Note that the query is conservative and may return additional objects that + don't have actual AABB overlap. This happens because the acceleration structures have some heuristic that + enlarges the AABBs a bit (extra margin and extruded along the velocity vector). + + Args: + aabb_min (np.array[float[3]]): minimum coordinates of the aabb + aabb_max (np.array[float[3]]): maximum coordinates of the aabb + + Returns: + list[int]: list of object unique ids. + """ + pass + + def get_aabb(self, body_id, link_id=-1): + """ + You can query the axis aligned bounding box (in world space) given an object unique id, and optionally a link + index. (when you don't pass the link index, or use -1, you get the AABB of the base). + + Args: + body_id (int): object unique id as returned by creation methods + link_id (int): link index in range [0..`getNumJoints(..)] + + Returns: + np.array[float[3]]: minimum coordinates of the axis aligned bounding box + np.array[float[3]]: maximum coordinates of the axis aligned bounding box + """ + pass + + def get_contact_points(self, body1, body2=None, link1_id=None, link2_id=None): + """ + Returns the contact points computed during the most recent call to `step`. + + Args: + body1 (int): only report contact points that involve body A + body2 (int, None): only report contact points that involve body B. Important: you need to have a valid + body A if you provide body B + link1_id (int, None): only report contact points that involve link index of body A + link2_id (int, None): only report contact points that involve link index of body B + + Returns: + list: + [0] int: contact flag (reserved) + [1] int: body unique id of body A + [2] int: body unique id of body B + [3] int: link index of body A, -1 for base + [4] int: link index of body B, -1 for base + [5] np.array[float[3]]: contact position on A, in Cartesian world coordinates + [6] np.array[float[3]]: contact position on B, in Cartesian world coordinates + [7] np.array[float[3]]: contact normal on B, pointing towards A + [8] float: contact distance, positive for separation, negative for penetration + [9] float: normal force applied during the last `step` + [10] float: lateral friction force in the first lateral friction direction (see next returned value) + [11] np.array[float[3]]: first lateral friction direction + [12] float: lateral friction force in the second lateral friction direction (see next returned value) + [13] np.array[float[3]]: second lateral friction direction + """ + # mjContact + # mj_contactForce + # check sim.data.contact = list of all detected contact + for contact in sim.data.contact: + geom_id1 = contact.geom1 + geom_id2 = contact.geom2 + + body_id1 = self.model.geom_bodyid[geom_id1] + body_id2 = self.model.geom_bodyid[geom_id2] + + midpos = contact.pos + dist = contact.dist + + frame = contact.frame + normal = frame[:3] + + frictions = contact.friction + lateral_1 = frictions[0] + lateral_2 = frictions[1] + spin = frictions[2] + roll_1 = frictions[3] + roll_2 = frictions[4] + pass + + def get_closest_points(self, body1, body2, distance, link1_id=None, link2_id=None): + """ + Computes the closest points, independent from `step`. This also lets you compute closest points of objects + with an arbitrary separating distance. In this query there will be no normal forces reported. + + Args: + body1 (int): only report contact points that involve body A + body2 (int): only report contact points that involve body B. Important: you need to have a valid body A + if you provide body B + distance (float): If the distance between objects exceeds this maximum distance, no points may be returned. + link1_id (int): only report contact points that involve link index of body A + link2_id (int): only report contact points that involve link index of body B + + Returns: + list: + int: contact flag (reserved) + int: body unique id of body A + int: body unique id of body B + int: link index of body A, -1 for base + int: link index of body B, -1 for base + np.array[float[3]]: contact position on A, in Cartesian world coordinates + np.array[float[3]]: contact position on B, in Cartesian world coordinates + np.array[float[3]]: contact normal on B, pointing towards A + float: contact distance, positive for separation, negative for penetration + float: normal force applied during the last `step`. Always equal to 0. + float: lateral friction force in the first lateral friction direction (see next returned value) + np.array[float[3]]: first lateral friction direction + float: lateral friction force in the second lateral friction direction (see next returned value) + np.array[float[3]]: second lateral friction direction + """ + pass + def ray_test(self, from_position, to_position): """ Performs a single raycast to find the intersection information of the first object hit. @@ -3005,6 +3421,57 @@ class Mujoco(Simulator): # TODO: get body_id and link_id from geom_id return geom_id, geom_id, fraction, position, normal + def ray_test_batch(self, from_positions, to_positions, parent_object_id=None, parent_link_id=None): + """Perform a batch of raycasts to find the intersection information of the first objects hit. + + This is similar to the ray_test, but allows you to provide an array of rays, for faster execution. The size of + 'rayFromPositions' needs to be equal to the size of 'rayToPositions'. You can one ray result per ray, even if + there is no intersection: you need to use the objectUniqueId field to check if the ray has hit anything: if + the objectUniqueId is -1, there is no hit. In that case, the 'hit fraction' is 1. + + Args: + from_positions (np.array[float[N,3]]): list of start points for each ray, in world coordinates + to_positions (np.array[float[N,3]]): list of end points for each ray in world coordinates + parent_object_id (int): ray from/to is in local space of a parent object + parent_link_id (int): ray from/to is in local space of a parent object + + Returns: + list: + int: object unique id of the hit object + int: link index of the hit object, or -1 if none/parent + float: hit fraction along the ray in range [0,1] along the ray. + np.array[float[3]]: hit position in Cartesian world coordinates + np.array[float[3]]: hit normal in Cartesian world coordinates + """ + pass + + def set_collision_filter_group_mask(self, body_id, link_id, filter_group, filter_mask): + """ + Enable/disable collision detection between groups of objects. Each body is part of a group. It collides with + other bodies if their group matches the mask, and vise versa. The following check is performed using the group + and mask of the two bodies involved. It depends on the collision filter mode. + + Args: + body_id (int): unique id of the body to be configured + link_id (int): link index of the body to be configured + filter_group (int): bitwise group of the filter + filter_mask (int): bitwise mask of the filter + """ + pass + + def set_collision_filter_pair(self, body1, body2, link1=-1, link2=-1, enable=True): + """ + Enable/disable collision between two bodies/links. + + Args: + body1 (int): unique id of body A to be filtered + body2 (int): unique id of body B to be filtered, A==B implies self-collision + link1 (int): link index of body A + link2 (int): link index of body B + enable (bool): True to enable collision, False to disable collision + """ + pass + ########################### # Kinematics and Dynamics # ########################### @@ -3152,7 +3619,7 @@ class Mujoco(Simulator): idx += 6 # self.model.dof_damping[idx] = joint_damping - def calculate_jacobian(self, body_id, link_id, local_position, q=None, dq=None, des_ddq=None): + def calculate_jacobian(self, body_id, link_id, local_position=None, q=None, dq=None, des_ddq=None): r""" Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that: @@ -3166,8 +3633,9 @@ class Mujoco(Simulator): Args: body_id (int): unique body id. link_id (int): link id. - local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link local - coordinates around its center of mass). If None, it will use the CoM position (in the link frame). + local_position (np.array[float[3]], None): the point on the specified link to compute the Jacobian (in + link local coordinates around its center of mass). If None, it will use the CoM position (in the link + frame). q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs. dq (np.array[float[N]]): joint velocities of size N, where N is the number of DoFs. des_ddq (np.array[float[N]]): desired joint accelerations of size N. @@ -3177,15 +3645,13 @@ class Mujoco(Simulator): number of columns depends if the base is fixed or floating. """ body = self._bodies[body_id] - if link_id < -1 or link_id > (body.num_bodies - 2): # -1 is for the base - raise ValueError("link_id should belong to [-1, `num_links-2`].") + link_id = self._check_link_id(body, link_id) + idx = body.b_idx0 + link_id # TODO: use q, dq, des_ddq by setting it in the data and then restoring the data - - idx = body.b_idx0 + 1 + link_id jacp, jacr = np.zeros(3 * self.model.nv), np.zeros(3 * self.model.nv) - if local_position is None: - local_position = np.zeros(3) + local_position = self.get_link_world_positions(body_id, link_ids=link_id-1) # in the cartesian world frame + # TODO: modify the local position mujoco.functions.mj_jac(self.model, self.sim.data, jacp, jacr, local_position, idx) jacp = jacp.reshape(3, self.model.nv)[:, body.v_idx0:body.v_idxf] jacr = jacr.reshape(3, self.model.nv)[:, body.v_idx0:body.v_idxf] @@ -3315,17 +3781,26 @@ class Mujoco(Simulator): body = self._bodies[body_id] # copy data - dest = mujoco.cymj.PyMjData() - mujoco.functions.mj_copyData(dest, self.model, self.sim.data) - dest.qpos[body.q_idx0:body.q_idxf] = q - dest.qvel[body.v_idx0:body.v_idxf] = dq - dest.qacc[body.v_idx0:body.v_idxf] = des_ddq + # dest = mujoco.cymj.PyMjData() + # mujoco.functions.mj_copyData(dest, self.model, self.sim.data) + # dest.qpos[body.q_idx0:body.q_idxf] = q + # dest.qvel[body.v_idx0:body.v_idxf] = dq + # dest.qacc[body.v_idx0:body.v_idxf] = des_ddq + + data = self._save_state() + self.sim.data.qpos[body.q_idx0:body.q_idxf] = q + self.sim.data.qvel[body.v_idx0:body.v_idxf] = dq + self.sim.data.qacc[body.v_idx0:body.v_idxf] = des_ddq # inverse dynamics - mujoco.functions.mj_inverse(self.model, dest) + mujoco.functions.mj_inverse(self.model, self.sim.data) # get resulting torques and return it - torques = dest.qfrc_applied[body.v_idx0:body.v_idxf] + # torques = self.sim.data.qfrc_applied[body.v_idx0:body.v_idxf] + torques = self.sim.data.qfrc_inverse[body.v_idx0:body.v_idxf] + + # restore data + self._load_state(data) return torques def calculate_forward_dynamics(self, body_id, q, dq, torques): @@ -3377,24 +3852,280 @@ class Mujoco(Simulator): body = self._bodies[body_id] # copy data and set q, dq, tau - dest = mujoco.cymj.PyMjData() - mujoco.functions.mj_copyData(dest, self.model, self.sim.data) - dest.qpos[body.q_idx0:body.q_idxf] = q - dest.qvel[body.v_idx0:body.v_idxf] = dq - dest.qfrc_applied[body.v_idx0:body.v_idxf] = torques + # dest = mujoco.cymj.PyMjData() + # mujoco.functions.mj_copyData(dest, self.model, self.sim.data) + # dest.qpos[body.q_idx0:body.q_idxf] = q + # dest.qvel[body.v_idx0:body.v_idxf] = dq + # dest.qfrc_applied[body.v_idx0:body.v_idxf] = torques + + data = self._save_state() + self.sim.data.qpos[body.q_idx0:body.q_idxf] = q + self.sim.data.qvel[body.v_idx0:body.v_idxf] = dq + self.sim.data.qfrc_applied[body.v_idx0:body.v_idxf] = torques # forward dynamics - mujoco.functions.mj_forward(self.model, dest) + mujoco.functions.mj_forward(self.model, self.sim.data) # get ddq and return it - qacc = dest.qacc[body.v_idx0:body.v_idxf] + qacc = self.sim.data.qacc[body.v_idx0:body.v_idxf] + + # restore data + self._load_state(data) return qacc ######### # Debug # ######### - # TODO + def add_user_debug_line(self, from_pos, to_pos, rgb_color=None, width=None, lifetime=None, + parent_object_id=None, + parent_link_id=None, line_id=None): + """Add a user debug line in the simulator. + + You can add a 3d line specified by a 3d starting point (from) and end point (to), a color [red,green,blue], + a line width and a duration in seconds. + + Args: + from_pos (np.array[float[3]]): starting point of the line in Cartesian world coordinates + to_pos (np.array[float[3]]): end point of the line in Cartesian world coordinates + rgb_color (np.array[float[3]]): RGB color (each channel in range [0,1]) + width (float): line width (limited by OpenGL implementation). + lifetime (float): use 0 for permanent line, or positive time in seconds (afterwards the line with be + removed automatically) + parent_object_id (int): draw line in local coordinates of a parent object. + parent_link_id (int): draw line in local coordinates of a parent link. + line_id (int): replace an existing line item (to avoid flickering of remove/add). + + Returns: + int: unique user debug line id. + """ + # mjr_drawPixels + pass + + def add_user_debug_text(self, text, position, rgb_color=None, size=None, lifetime=None, orientation=None, + parent_object_id=None, parent_link_id=None, text_id=None): + """ + Add 3D text at a specific location using a color and size. + + Args: + text (str): text. + position (np.array[float[3]]): 3d position of the text in Cartesian world coordinates. + rgb_color (list/tuple of 3 floats): RGB color; each component in range [0..1] + size (float): text size + lifetime (float): use 0 for permanent text, or positive time in seconds (afterwards the text with be + removed automatically) + orientation (np.array[float[4]]): By default, debug text will always face the camera, automatically + rotation. By specifying a text orientation (quaternion), the orientation will be fixed in world space + or local space (when parent is specified). Note that a different implementation/shader is used for + camera facing text, with different appearance: camera facing text uses bitmap fonts, text with + specified orientation uses TrueType font. + parent_object_id (int): draw text in local coordinates of a parent object. + parent_link_id (int): draw text in local coordinates of a parent link. + text_id (int): replace an existing text item (to avoid flickering of remove/add). + + Returns: + int: unique user debug text id. + """ + # mjr_text + pass + + def add_user_debug_parameter(self, name, min_range, max_range, start_value): + """ + Add custom sliders to tune parameters. + + Args: + name (str): name of the parameter. + min_range (float): minimum value. + max_range (float): maximum value. + start_value (float): starting value. + + Returns: + int: unique user debug parameter id. + """ + pass + + def read_user_debug_parameter(self, parameter_id): + """ + Read the value of the parameter / slider. + + Args: + parameter_id: unique user debug parameter id. + + Returns: + float: reading of the parameter. + """ + pass + + def remove_user_debug_item(self, item_id): + """ + Remove the specified user debug item (line, text, parameter) from the simulator. + + Args: + item_id (int): unique id of the debug item to be removed (line, text etc) + """ + pass + + def remove_all_user_debug_items(self): + """ + Remove all user debug items from the simulator. + """ + pass + + def set_debug_object_color(self, object_id, link_id, rgb_color=(1, 0, 0)): + """ + Override the color of a specific object and link. + + Args: + object_id (int): unique object id. + link_id (int): link id. + rgb_color (float[3]): RGB debug color. + """ + pass + + def add_user_data(self, object_id, key, value): + """ + Add user data (at the moment text strings) attached to any link of a body. You can also override a previous + given value. You can add multiple user data to the same body/link. + + Args: + object_id (int): unique object/link id. + key (str): key string. + value (str): value string. + + Returns: + int: user data id. + """ + pass + + def num_user_data(self, object_id): + """ + Return the number of user data associated with the specified object/link id. + + Args: + object_id (int): unique object/link id. + + Returns: + int: the number of user data + """ + pass + + def get_user_data(self, user_data_id): + """ + Get the specified user data value. + + Args: + user_data_id (int): unique user data id. + + Returns: + str: value string. + """ + pass + + def get_user_data_id(self, object_id, key): + """ + Get the specified user data id. + + Args: + object_id (int): unique object/link id. + key (str): key string. + + Returns: + int: user data id. + """ + pass + + def get_user_data_info(self, object_id, index): + """ + Get the user data info associated with the given object and index. + + Args: + object_id (int): unique object id. + index (int): index (should be between [0, self.num_user_data(object_id)]). + + Returns: + int: user data id. + str: key. + int: body id. + int: link index + int: visual shape index. + """ + pass + + def remove_user_data(self, user_data_id): + """ + Remove the specified user data. + + Args: + user_data_id (int): user data id. + """ + pass + + def sync_user_data(self): + """ + Synchronize the user data. + """ + pass + + def configure_debug_visualizer(self, flag, enable): + """Configure the debug visualizer camera. + + Configure some settings of the built-in OpenGL visualizer, such as enabling or disabling wireframe, + shadows and GUI rendering. + + Args: + flag (int): The feature to enable or disable, such as + COV_ENABLE_WIREFRAME (=3): show/hide the collision wireframe + COV_ENABLE_SHADOWS (=2): show/hide shadows + COV_ENABLE_GUI (=1): enable/disable the GUI + COV_ENABLE_VR_PICKING (=5): enable/disable VR picking + COV_ENABLE_VR_TELEPORTING (=4): enable/disable VR teleporting + COV_ENABLE_RENDERING (=7): enable/disable rendering + COV_ENABLE_TINY_RENDERER (=12): enable/disable tiny renderer + COV_ENABLE_VR_RENDER_CONTROLLERS (=6): render VR controllers + COV_ENABLE_KEYBOARD_SHORTCUTS (=9): enable/disable keyboard shortcuts + COV_ENABLE_MOUSE_PICKING (=10): enable/disable mouse picking + COV_ENABLE_Y_AXIS_UP (Z is default world up axis) (=11): enable/disable Y axis up + COV_ENABLE_RGB_BUFFER_PREVIEW (=13): enable/disable RGB buffer preview + COV_ENABLE_DEPTH_BUFFER_PREVIEW (=14): enable/disable Depth buffer preview + COV_ENABLE_SEGMENTATION_MARK_PREVIEW (=15): enable/disable segmentation mark preview + enable (bool): False (disable) or True (enable) + """ + pass + + def get_debug_visualizer(self): + """Get information about the debug visualizer camera. + + Returns: + float: width of the visualizer camera + float: height of the visualizer camera + np.array[float[4,4]],4]: view matrix [4,4] + np.array[float[4,4]],4]: perspective projection matrix [4,4] + np.array[float[3]]: camera up vector expressed in the Cartesian world space + np.array[float[3]]: forward axis of the camera expressed in the Cartesian world space + np.array[float[3]]: This is a horizontal vector that can be used to generate rays (for mouse picking or + creating a simple ray tracer for example) + np.array[float[3]]: This is a vertical vector that can be used to generate rays (for mouse picking or + creating a simple ray tracer for example) + float: yaw angle (in radians) of the camera, in Cartesian local space coordinates + float: pitch angle (in radians) of the camera, in Cartesian local space coordinates + float: distance between the camera and the camera target + np.array[float[3]]: target of the camera, in Cartesian world space coordinates + """ + pass + + def reset_debug_visualizer(self, distance, yaw, pitch, target_position): + """Reset the debug visualizer camera. + + Reset the 3D OpenGL debug visualizer camera distance (between eye and camera target position), camera yaw and + pitch and camera target position + + Args: + distance (float): distance from eye to camera target position + yaw (float): camera yaw angle (in radians) left/right + pitch (float): camera pitch angle (in radians) up/down + target_position (np.array[float[3]]): target focus point of the camera + """ + pass ############################ # Events (mouse, keyboard) # diff --git a/pyrobolearn/simulators/raisim.py b/pyrobolearn/simulators/raisim.py index ac21ed2..7246921 100644 --- a/pyrobolearn/simulators/raisim.py +++ b/pyrobolearn/simulators/raisim.py @@ -48,6 +48,8 @@ except ImportError as e: # import PRL simulator from pyrobolearn.simulators.simulator import Simulator from pyrobolearn.utils.decorator import keyboard_interrupt +from pyrobolearn.utils.mesh import convert_mesh +from pyrobolearn.utils.parsers.robots import URDFParser __author__ = "Brian Delhaisse" @@ -354,6 +356,22 @@ class Raisim(Simulator): camera.pitch(1.2) camera.yaw(0.6, raisim.ogre.Node.TransformSpace.TS_WORLD) + ################# + # utils methods # + ################# + + @staticmethod + def _convert_wxyz_to_xyzw(q): + """Convert a quaternion in the (w,x,y,z) format to (x,y,z,w).""" + q = np.asarray(q) + return np.roll(q, shift=-1, axis=q.ndim - 1) + + @staticmethod + def _convert_xyzw_to_wxyz(q): + """Convert a quaternion in the (x,y,z,w) format to (w,x,y,z).""" + q = np.asarray(q) + return np.roll(q, shift=1, axis=q.ndim - 1) + ############# # Simulator # ############# @@ -399,8 +417,8 @@ class Raisim(Simulator): # update visualization counter self.visualization_cnt += 1 - if sleep_time: - time.sleep(sleep_time) + # if sleep_time: + # time.sleep(sleep_time) def is_rendering(self): """Return True if the simulator is in the render mode.""" @@ -516,6 +534,19 @@ class Raisim(Simulator): # Loading URDFs, SDFs, MJCFs, meshes # ###################################### + @staticmethod + def _convert_mesh(filename, format='obj'): + extension = filename.split('.')[-1] + if extension.lower() != format: # if different file format than obj convert it + basename = os.path.basename(filename) + basename_without_extension = ''.join(basename.split('.')[:-1]) + # dirname = os.path.dirname(os.path.abspath(__file__)) + '/meshes/' # Raisim uses relative paths + new_filename = basename_without_extension + '.' + format + if not os.path.isfile(new_filename): + convert_mesh(filename, 'meshes/' + new_filename, library='pyassimp') + return True, new_filename + return False, filename + def load_urdf(self, filename, position, orientation=None, use_fixed_base=0, scale=1.0, *args, **kwargs): """Load a URDF file in the simulator. @@ -531,6 +562,31 @@ class Raisim(Simulator): Returns: int (non-negative): unique id associated to the load model. """ + # parse the URDF file + urdf_parser = URDFParser(filename=filename) + tree = urdf_parser.tree + + # Raisim only accepts collision bodies in the obj format, so check that each mesh is in the correct format. + # If not, convert them. + urdf_changed = False + for body in tree.bodies.values(): + for visual in body.visuals: + if visual.dtype == 'mesh': + urdf_changed, new_filename = self._convert_mesh(visual.filename) + visual.filename = new_filename + for collision in body.collisions: + if collision.dtype == 'mesh': + urdf_changed, new_filename = self._convert_mesh(collision.filename) + collision.filename = new_filename + + # if we had to convert some meshes, just create a new URDF with the converted meshes + if urdf_changed: + root = urdf_parser.generate(tree) + basename = os.path.basename(filename) + dirname = os.path.dirname(os.path.abspath(__file__)) + '/meshes/' + filename = dirname + 'prl_generated_' + basename + urdf_parser.write(filename, root=root) + # load body body = self.world.add_articulated_system(filename) @@ -730,7 +786,14 @@ class Raisim(Simulator): Args: body_id (int): unique body id. """ - pass + body = self._bodies.pop(body_id) + + # remove body from the world + self.world.remove_object(body) + + # remove body from visualization + if self._render: + self.visualizer.remove(body) def num_bodies(self): """Return the number of bodies present in the simulator. @@ -738,7 +801,9 @@ class Raisim(Simulator): Returns: int: number of bodies """ - pass + # return len(self._bodies) + # return len(self.world.get_object_list) + return self.world.get_configuration_number() def get_body_info(self, body_id): """Get the specified body information. @@ -862,10 +927,1061 @@ class Raisim(Simulator): # Objects # ########### + def get_mass(self, body_id): + """ + Return the total mass of the robot (=sum of all mass links). + + Args: + body_id (int): unique object id, as returned from `load_urdf`. + + Returns: + float: total mass of the robot [kg] + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_mass(0) + return sum(body.get_masses()) + + def get_base_mass(self, body_id): + """Return the base mass of the robot. + + Args: + body_id (int): unique object id. + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_mass(0) + return body.get_masses()[0] + + def get_base_name(self, body_id): + """ + Return the base name. + + Args: + body_id (int): unique object id. + + Returns: + str: base name + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_name() + return body.get_body_names()[0] # body.get_name() + + def get_center_of_mass_position(self, body_id, link_ids=None): + """ + Return the center of mass position. + + Args: + body_id (int): unique body id. + link_ids (list[int]): link ids associated with the given body id. If None, it will take all the links + of the specified body. + + Returns: + np.array[float[3]]: center of mass position in the Cartesian world coordinates + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_com_position() + return body.get_composite_com() + + def get_center_of_mass_velocity(self, body_id, link_ids=None): + """ + Return the center of mass linear velocity. + + Args: + body_id (int): unique body id. + link_ids (list[int]): link ids associated with the given body id. If None, it will take all the links + of the specified body. + + Returns: + np.array[float[3]]: center of mass linear velocity. + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_linear_velocity() + return body.get_world_linear_velocity(body_id=body_id, body_pos=np.zeros(3)) # TODO: correct body_id + + def get_base_pose(self, body_id): + """ + Get the current position and orientation of the base (or root link) of the body in Cartesian world coordinates. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[float[3]]: base position + np.array[float[4]]: base orientation (quaternion [x,y,z,w]) + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_position(), self._convert_wxyz_to_xyzw(body.get_quaternion()) + pos, quat = body.get_body_pose(0) + return pos, self._convert_wxyz_to_xyzw(quat) + + def get_base_position(self, body_id): + """ + Return the base position of the specified body. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[float[3]]: base position. + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_position() + return body.get_world_position(0) + + def get_base_orientation(self, body_id): + """ + Get the base orientation of the specified body. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[float[4]]: base orientation in the form of a quaternion (x,y,z,w) + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return self._convert_wxyz_to_xyzw(body.get_quaternion()) + return self._convert_wxyz_to_xyzw(body.get_base_quaternion()) + + def reset_base_pose(self, body_id, position, orientation): + """ + Reset the base position and orientation of the specified object id. + + Args: + body_id (int): unique object id. + position (np.array[float[3]]): new base position. + orientation (np.array[float[4]]): new base orientation (expressed as a quaternion [x,y,z,w]) + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + body.set_position(position) + body.set_orientation(self._convert_xyzw_to_wxyz(orientation)) + else: + body.set_base_position(position) + body.set_base_orientation(self._convert_xyzw_to_wxyz(orientation)) + + def reset_base_position(self, body_id, position): + """ + Reset the base position of the specified body/object id while preserving its orientation. + + Args: + body_id (int): unique object id. + position (np.array[float[3]]): new base position. + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + body.set_position(position) + else: + body.set_base_position(position) + + def reset_base_orientation(self, body_id, orientation): + """ + Reset the base orientation of the specified body/object id while preserving its position. + + Args: + body_id (int): unique object id. + orientation (np.array[float[4]]): new base orientation (expressed as a quaternion [x,y,z,w]) + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + body.set_orientation(self._convert_xyzw_to_wxyz(orientation)) + else: + body.set_base_orientation(self._convert_xyzw_to_wxyz(orientation)) + + def get_base_velocity(self, body_id): + """ + Return the base linear and angular velocities. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[float[3]]: linear velocity of the base in Cartesian world space coordinates + np.array[float[3]]: angular velocity of the base in Cartesian world space coordinates + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_linear_velocity(), body.get_angular_velocity() + return body.get_world_linear_velocity(0), body.get_world_angular_velocity(0) + + def get_base_linear_velocity(self, body_id): + """ + Return the linear velocity of the base. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[float[3]]: linear velocity of the base in Cartesian world space coordinates + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_linear_velocity() + return body.get_world_linear_velocity(0) + + def get_base_angular_velocity(self, body_id): + """ + Return the angular velocity of the base. + + Args: + body_id (int): object unique id, as returned from `load_urdf`. + + Returns: + np.array[float[3]]: angular velocity of the base in Cartesian world space coordinates + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_angular_velocity() + return body.get_world_angular_velocity(0) + + def reset_base_velocity(self, body_id, linear_velocity=None, angular_velocity=None): + """ + Reset the base velocity. + + Args: + body_id (int): unique object id. + linear_velocity (np.array[float[3]]): new linear velocity of the base. + angular_velocity (np.array[float[3]]): new angular velocity of the base. + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + body.set_velocity(linear_velocity, angular_velocity) + else: + # TODO: request feature on Raisim github + pass + + def reset_base_linear_velocity(self, body_id, linear_velocity): + """ + Reset the base linear velocity. + + Args: + body_id (int): unique object id. + linear_velocity (np.array[float[3]]): new linear velocity of the base + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + body.set_velocity(linear_velocity, np.zeros(3)) + else: + # TODO: request feature on Raisim github + pass + + def reset_base_angular_velocity(self, body_id, angular_velocity): + """ + Reset the base angular velocity. + + Args: + body_id (int): unique object id. + angular_velocity (np.array[float[3]]): new angular velocity of the base + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + body.set_velocity(np.zeros(3), angular_velocity) + else: + # TODO: request feature on Raisim github + pass + + def get_base_acceleration(self, body_id): + """ + Get the base acceleration. This is only valid if the simulator `supports_acceleration`. + + Args: + body_id (int): unique object id. + + Returns: + np.array[float[3]]: linear acceleration [m/s^2] + np.array[float[3]]: angular acceleration [rad/s^2] + """ + pass # Raisim does not support accelerations + + def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.), + frame=Simulator.LINK_FRAME): + """ + Apply the specified external force on the specified position on the body / link. + + Args: + body_id (int): unique body id. + link_id (int): unique link id. If -1, it will be the base. + force (np.array[float[3]]): external force to be applied. + position (np.array[float[3]]): position on the link where the force is applied. See `flags` for coordinate + systems. If None, it is the center of mass of the body (or the link if specified). + frame (int): if frame = 1, then the force / position is described in the link frame. If frame = 2, they + are described in the world frame. + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + body.setExternalForce(link_id + 1, force) + else: + if frame == Simulator.LINK_FRAME: + frame = raisim.ArticulatedSystem.Frame.BODY_FRAME + elif frame == Simulator.WORLD_FRAME: + frame = raisim.ArticulatedSystem.Frame.WORLD_FRAME + else: + raise ValueError("Unknown specified frame.") + body.setExternalForce(link_id + 1, frame, force, frame, position) + + def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.), frame=1): + """ + Apply an external torque on a body, or a link of the body. Note that after each simulation step, the external + torques are cleared to 0. + + Args: + body_id (int): unique body id. + link_id (int): link id to apply the torque, if -1 it will apply the torque on the base + torque (float[3]): Cartesian torques to be applied on the body + frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for + Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates. + """ + body = self._bodies[body_id] + body.setExternalTorque(link_id + 1, torque) + ############################# # Robots (joints and links) # ############################# + def num_joints(self, body_id): + """ + Return the total number of joints of the specified body. This is the same as calling `num_links`. + + Args: + body_id (int): unique body id. + + Returns: + int: number of joints with the associated body id. + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return 0 + return len(body.get_body_names()) + + def num_actuated_joints(self, body_id): + """ + Return the total number of actuated joints associated with the given body id. + + Args: + body_id (int): unique body id. + + Returns: + int: number of actuated joints of the specified body. + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return 0 + return body.get_num_dof() + + def num_links(self, body_id): + """ + Return the total number of links of the specified body. This is the same as calling `num_joints`. + + Args: + body_id (int): unique body id. + + Returns: + int: number of links with the associated body id. + """ + return self.num_joints(body_id) + + def get_joint_info(self, body_id, joint_id): + """ + Return information about the given joint about the specified body. + + Note that this method returns a lot of information, so specific methods have been implemented that return + only the desired information. Also, note that we do not convert the data here. + + Args: + body_id (int): unique body id. + joint_id (int): joint id is included in [0..`num_joints(body_id)`]. + + Returns: + dict, list: joint info + """ + pass + + def get_joint_state(self, body_id, joint_id): + """ + Get the joint state. + + Args: + body_id (int): unique body id. + joint_id (int): joint index in range [0..num_joints(body_id)] + + Returns: + float: The position value of this joint. + float: The velocity value of this joint. + np.array[float[6]]: These are the joint reaction forces, if a torque sensor is enabled for this joint it is + [Fx, Fy, Fz, Mx, My, Mz]. Without torque sensor, it is [0, 0, 0, 0, 0, 0]. + float: This is the motor torque applied during the last stepSimulation. Note that this only applies in + VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor torque + is exactly what you provide, so there is no need to report it separately. + """ + pass + + def get_joint_states(self, body_id, joint_ids): + """ + Get the joint state of the specified joints. + + Args: + body_id (int): unique body id. + joint_ids (list[int]): list of joint ids. + + Returns: + list: + float: The position value of this joint. + float: The velocity value of this joint. + np.array[float[6]]: These are the joint reaction forces, if a torque sensor is enabled for this joint + it is [Fx, Fy, Fz, Mx, My, Mz]. Without torque sensor, it is [0, 0, 0, 0, 0, 0]. + float: This is the motor torque applied during the last `step`. Note that this only applies in + VELOCITY_CONTROL and POSITION_CONTROL. If you use TORQUE_CONTROL then the applied joint motor + torque is exactly what you provide, so there is no need to report it separately. + """ + pass + + 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. + + Args: + body_id (int): unique body id. + joint_id (int): joint index in range [0..num_joints(body_id)] + 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]) + """ + pass + + def enable_joint_force_torque_sensor(self, body_id, joint_ids, enable=True): + """ + You can enable or disable a joint force/torque sensor in each joint. + + Args: + body_id (int): body unique id. + joint_ids (int, int[N]): joint index in range [0..num_joints(body_id)], or list of joint ids. + enable (bool): True to enable, False to disable the force/torque sensor + """ + pass + + def set_joint_motor_control(self, body_id, joint_ids, control_mode=2, positions=None, + velocities=None, forces=None, kp=None, kd=None, max_velocity=None): + r""" + Set the joint motor control. + + In position control: + .. math:: error = Kp (x_{des} - x) + Kd (\dot{x}_{des} - \dot{x}) + + In velocity control: + .. math:: error = \dot{x}_{des} - \dot{x} + + Note that the maximum forces and velocities are not automatically used for the different control schemes. + + Args: + body_id (int): body unique id. + joint_ids (int): joint/link id, or list of joint ids. + control_mode (int): POSITION_CONTROL (=2) (which is in fact CONTROL_MODE_POSITION_VELOCITY_PD), + VELOCITY_CONTROL (=0), TORQUE_CONTROL (=1) and PD_CONTROL (=3). + positions (float, np.array[float[N]]): target joint position(s) (used in POSITION_CONTROL). + velocities (float, np.array[float[N]]): target joint velocity(ies). In VELOCITY_CONTROL and + POSITION_CONTROL, the target velocity(ies) is(are) the desired velocity of the joint. Note that the + target velocity(ies) is(are) not the maximum joint velocity(ies). In PD_CONTROL and + POSITION_CONTROL/CONTROL_MODE_POSITION_VELOCITY_PD, the final target velocities are computed using: + `kp*(erp*(desiredPosition-currentPosition)/dt)+currentVelocity+kd*(m_desiredVelocity - currentVelocity)` + forces (float, list[float]): in POSITION_CONTROL and VELOCITY_CONTROL, these are the maximum motor + forces used to reach the target values. In TORQUE_CONTROL these are the forces / torques to be applied + each simulation step. + kp (float, list[float]): position (stiffness) gain(s) (used in POSITION_CONTROL). + kd (float, list[float]): velocity (damping) gain(s) (used in POSITION_CONTROL). + max_velocity (float): in POSITION_CONTROL this limits the velocity to a maximum. + """ + pass + + def get_link_state(self, body_id, link_id, compute_velocity=False, compute_forward_kinematics=False): + """ + Get the state of the associated link. + + Args: + body_id (int): body unique id. + link_id (int): link index. + compute_velocity (bool): If True, the Cartesian world velocity will be computed and returned. + compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed + using forward kinematics. + + Returns: + np.array[float[3]]: Cartesian world position of CoM + np.array[float[4]]: Cartesian world orientation of CoM, in quaternion [x,y,z,w] + np.array[float[3]]: local position offset of inertial frame (center of mass) expressed in the URDF link + frame + np.array[float[4]]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed in + URDF link frame + np.array[float[3]]: world position of the URDF link frame + np.array[float[4]]: world orientation of the URDF link frame + np.array[float[3]]: Cartesian world linear velocity. Only returned if `compute_velocity` is True. + np.array[float[3]]: Cartesian world angular velocity. Only returned if `compute_velocity` is True. + """ + pass + + def get_link_states(self, body_id, link_ids, compute_velocity=False, compute_forward_kinematics=False): + """ + Get the state of the associated links. + + Args: + body_id (int): body unique id. + link_ids (list[int]): list of link index. + compute_velocity (bool): If True, the Cartesian world velocity will be computed and returned. + compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed + using forward kinematics. + + Returns: + list: + np.array[float[3]]: Cartesian position of CoM + np.array[float[4]]: Cartesian orientation of CoM, in quaternion [x,y,z,w] + np.array[float[3]]: local position offset of inertial frame (center of mass) expressed in the URDF + link frame + np.array[float[4]]: local orientation (quaternion [x,y,z,w]) offset of the inertial frame expressed + in URDF link frame + np.array[float[3]]: world position of the URDF link frame + np.array[float[4]]: world orientation of the URDF link frame + np.array[float[3]]: Cartesian world linear velocity. Only returned if `compute_velocity` is True. + np.array[float[3]]: Cartesian world angular velocity. Only returned if `compute_velocity` is True. + """ + pass + + def get_link_names(self, body_id, link_ids): + """ + Return the name of the given link(s). + + Args: + body_id (int): unique body id. + link_ids (int, list[int]): link id, or list of link ids. + + Returns: + if 1 link: + str: link name + if multiple links: + str[N]: link names + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_name() + names = body.get_body_names() + if isinstance(link_ids, int): + return names[link_ids] + return np.asarray(names)[link_ids] + + def get_link_masses(self, body_id, link_ids): + """ + Return the mass of the given link(s). + + Args: + body_id (int): unique body id. + link_ids (int, list[int]): link id, or list of link ids. + + Returns: + if 1 link: + float: mass of the given link + else: + float[N]: mass of each link + """ + body = self._bodies[body_id] + if isinstance(body, raisim.SingleBodyObject): + return body.get_mass(0) + masses = body.get_masses() + if isinstance(link_ids, int): + return masses[link_ids] + return np.asarray(masses)[link_ids] + + def get_link_frames(self, body_id, link_ids): + r""" + Return the link world frame position(s) and orientation(s). + + Args: + body_id (int): body id. + link_ids (int, int[N]): link id, or list of desired link ids. + + Returns: + if 1 link: + np.array[float[3]]: the link frame position in the world space + np.array[float[4]]: Cartesian orientation of the link frame [x,y,z,w] + if multiple links: + np.array[float[N,3]]: link frame position of each link in world space + np.array[float[N,4]]: orientation of each link frame [x,y,z,w] + """ + body = self._bodies[body_id] + # get_frame_world_position + # get_frame_world_quaternion + + def get_link_world_positions(self, body_id, link_ids): + """ + Return the CoM position (in the Cartesian world space coordinates) of the given link(s). + + 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]]: the link CoM position in the world space + if multiple links: + np.array[float[N,3]]: CoM position of each link in world space + """ + body = self._bodies[body_id] + # get_link_coms # in body frame + # get_frame_world_position + + def get_link_positions(self, body_id, link_ids): + pass + + def get_link_world_orientations(self, body_id, link_ids): + """ + Return the CoM orientation (in the Cartesian world space) of the given link(s). + + 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[4]]: Cartesian orientation of the link CoM (x,y,z,w) + if multiple links: + np.array[float[N,4]]: CoM orientation of each link (x,y,z,w) + """ + pass + + def get_link_orientations(self, body_id, link_ids): + pass + + def get_link_world_linear_velocities(self, body_id, link_ids): + """ + Return the linear velocity 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 velocity of the link in the Cartesian world space + if multiple links: + np.array[float[N,3]]: linear velocity of each link + """ + body = self._bodies[body_id] + # get_frame_linear_velocity + # get_world_linear_velocity + + def get_link_world_angular_velocities(self, body_id, link_ids): + """ + Return the angular velocity 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 velocity of the link in the Cartesian world space + if multiple links: + np.array[float[N,3]]: angular velocity of each link + """ + body = self._bodies[body_id] + # get_frame_angular_velocity + # get_world_angular_velocity + + def get_link_world_velocities(self, body_id, link_ids): + """ + Return the linear and angular velocities (expressed in the Cartesian world space coordinates) for the given + link(s). + + 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[6]]: linear and angular velocity of the link in the Cartesian world space + if multiple links: + np.array[float[N,6]]: linear and angular velocity of each link + """ + body = self._bodies[body_id] + # get_frame_linear_velocity + # get_world_linear_velocity + # get_frame_angular_velocity + # get_world_angular_velocity + + 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 # Raisim does not support accelerations + + 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 # Raisim does not support accelerations + + 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 + link(s). This is only valid if the simulator `supports_acceleration`. + + 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[6]]: linear and angular acceleration of the link in the Cartesian world space + if multiple links: + np.array[float[N,6]]: linear and angular acceleration of each link + """ + pass # Raisim does not support accelerations + + def get_q_indices(self, body_id, joint_ids): + """ + Get the corresponding q index of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + int: q index + if multiple joints: + list[int]: q indices + """ + pass + + def get_actuated_joint_ids(self, body_id): + """ + Get the actuated joint ids associated with the given body id. + + Args: + body_id (int): unique body id. + + Returns: + list[int]: actuated joint ids. + """ + pass + + def get_joint_names(self, body_id, joint_ids): + """ + Return the name of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + str: name of the joint + if multiple joints: + str[N]: name of each joint + """ + pass + + def get_joint_type_ids(self, body_id, joint_ids): + """ + Get the joint type ids. + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + int: joint type id. + if multiple joints: list of above + """ + pass + + def get_joint_type_names(self, body_id, joint_ids): + """ + Get joint type names. + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + str: joint type name. + if multiple joints: list of above + """ + pass + + def get_joint_dampings(self, body_id, joint_ids): + """ + Get the damping coefficient of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: damping coefficient of the given joint + if multiple joints: + np.array[float[N]]: damping coefficient for each specified joint + """ + pass + + def get_joint_frictions(self, body_id, joint_ids): + """ + Get the friction coefficient of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: friction coefficient of the given joint + if multiple joints: + np.array[float[N]]: friction coefficient for each specified joint + """ + pass + + def get_joint_limits(self, body_id, joint_ids): + """ + Get the joint limits of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + np.array[float[2]]]: lower and upper limit + if multiple joints: + np.array[N,2]: lower and upper limit for each specified joint + """ + pass + + def get_joint_max_forces(self, body_id, joint_ids): + """ + Get the maximum force that can be applied on the given joint(s). + + Warning: Note that this is not automatically used in position, velocity, or torque control. + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: maximum force [N] + if multiple joints: + np.array[float[N]]: maximum force for each specified joint [N] + """ + pass + + def get_joint_max_velocities(self, body_id, joint_ids): + """ + Get the maximum velocity that can be applied on the given joint(s). + + Warning: Note that this is not automatically used in position, velocity, or torque control. + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: maximum velocity [rad/s] + if multiple joints: + np.array[float[N]]: maximum velocities for each specified joint [rad/s] + """ + pass + + def get_joint_axes(self, body_id, joint_ids): + """ + Get the joint axis about the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + np.array[float[3]]: joint axis + if multiple joint: + np.array[float[N,3]]: list of joint axis + """ + pass + + def set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None): + """ + Set the position of the given joint(s) (using position control). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + positions (float, np.array[float[N]]): desired position, or list of desired positions [rad] + velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + kps (None, float, np.array[float[N]]): position gain(s) + kds (None, float, np.array[float[N]]): velocity gain(s) + forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values. + """ + pass + + def get_joint_positions(self, body_id, joint_ids): + """ + Get the position of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint position [rad] + if multiple joints: + np.array[float[N]]: joint positions [rad] + """ + pass + + def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None): + """ + Set the velocity of the given joint(s) (using velocity control). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + max_force (None, float, np.array[float[N]]): maximum motor forces/torques + """ + pass + + def get_joint_velocities(self, body_id, joint_ids): + """ + Get the velocity of the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint velocity [rad/s] + if multiple joints: + np.array[float[N]]: joint velocities [rad/s] + """ + pass + + def set_joint_accelerations(self, body_id, joint_ids, accelerations, q=None, dq=None): + """ + Set the acceleration of the given joint(s) (using force control). This is achieved by performing inverse + dynamic which given the joint accelerations compute the joint torques to be applied. + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + accelerations (float, np.array[float[N]]): desired joint acceleration, or list of desired joint + accelerations [rad/s^2] + """ + pass + + def get_joint_accelerations(self, body_id, joint_ids): # , q=None, dq=None): + """ + Get the acceleration of the specified joint(s). This is only valid if the simulator `supports_acceleration`. + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint acceleration [rad/s^2] + if multiple joints: + np.array[float[N]]: joint accelerations [rad/s^2] + """ + pass + + def set_joint_torques(self, body_id, joint_ids, torques): + """ + Set the torque/force to the given joint(s) (using force/torque control). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + torques (float, list[float], np.array[float]): desired torque(s) to apply to the joint(s) [N]. + """ + pass + + def get_joint_torques(self, body_id, joint_ids): + """ + Get the applied torque(s) on the given joint(s). + + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: torque [Nm] + if multiple joints: + np.array[float[N]]: torques associated to the given joints [Nm] + """ + pass + + def get_joint_reaction_forces(self, body_id, joint_ids): + """Return the joint reaction forces at the given joint. Note that the torque sensor must be enabled, otherwise + it will always return [0,0,0,0,0,0]. + + Args: + body_id (int): unique body id. + joint_ids (int, int[N]): joint id, or list of joint ids + + Returns: + if 1 joint: + np.array[float[6]]: joint reaction force (fx,fy,fz,mx,my,mz) [N,Nm] + if multiple joints: + np.array[float[N,6]]: joint reaction forces [N, Nm] + """ + pass + + def get_joint_powers(self, body_id, joint_ids): + """Return the applied power at the given joint(s). Power = torque * velocity. + + Args: + body_id (int): unique body id. + joint_ids (int, int[N]): joint id, or list of joint ids + + Returns: + if 1 joint: + float: joint power [W] + if multiple joints: + np.array[float[N]]: power at each joint [W] + """ + pass + ################# # Visualization # ################# @@ -874,6 +1990,256 @@ class Raisim(Simulator): # Collisions # ############## + ########################### + # Kinematics and Dynamics # + ########################### + + def get_dynamics_info(self, body_id, link_id=-1): + """ + Get dynamic information about the mass, center of mass, friction and other properties of the base and links. + + Args: + body_id (int): body/object unique id. + link_id (int): link/joint index or -1 for the base. + + Returns: + float: mass in kg + float: lateral friction coefficient + np.array[float[3]]: local inertia diagonal. Note that links and base are centered around the center of + mass and aligned with the principal axes of inertia. + np.array[float[3]]: position of inertial frame in local coordinates of the joint frame + np.array[float[4]]: orientation of inertial frame in local coordinates of joint frame + float: coefficient of restitution + float: rolling friction coefficient orthogonal to contact normal + float: spinning friction coefficient around contact normal + float: damping of contact constraints. -1 if not available. + float: stiffness of contact constraints. -1 if not available. + """ + pass + + def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None, + rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None, + contact_stiffness=None, contact_damping=None, friction_anchor=None, + local_inertia_diagonal=None, inertia_position=None, inertia_orientation=None, + joint_damping=None, joint_friction=None): + """ + Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc. + + Args: + body_id (int): object unique id, as returned by `load_urdf`, etc. + link_id (int): link index or -1 for the base. + mass (float): change the mass of the link (or base for link index -1) + lateral_friction (float): lateral (linear) contact friction + spinning_friction (float): torsional friction around the contact normal + rolling_friction (float): torsional friction orthogonal to contact normal + restitution (float): bounciness of contact. Keep it a bit less than 1. + linear_damping (float): linear damping of the link (0.04 by default) + angular_damping (float): angular damping of the link (0.04 by default) + contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping` + contact_damping (float): damping of the contact constraints for this body/link. Used together with + `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact + section. + friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by + default, unless set in the URDF contact section) + local_inertia_diagonal (np.array[float[3]]): diagonal elements of the inertia tensor. Note that the base + and links are centered around the center of mass and aligned with the principal axes of inertia so + there are no off-diagonal elements in the inertia tensor. + inertia_position (np.array[float[3]]): new inertia position with respect to the link frame. + inertia_orientation (np.array[float[4]]): new inertia orientation (expressed as a quaternion [x,y,z,w] + with respect to the link frame. + joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF + joint damping field. Keep the value close to 0. + `joint_damping_force = -damping_coefficient * joint_velocity`. + joint_friction (float): joint friction coefficient. + """ + pass + + def calculate_jacobian(self, body_id, link_id, local_position, q, dq, des_ddq): + r""" + Return the full geometric Jacobian matrix :math:`J(q) = [J_{lin}(q), J_{ang}(q)]^T`, such that: + + .. math:: v = [\dot{p}, \omega]^T = J(q) \dot{q} + + where :math:`\dot{p}` is the Cartesian linear velocity of the link, and :math:`\omega` is its angular velocity. + + Warnings: if we have a floating base then the Jacobian will also include columns corresponding to the root + link DoFs (at the beginning). If it is a fixed base, it will only have columns associated with the joints. + + Args: + body_id (int): unique body id. + link_id (int): link id. + local_position (np.array[float[3]]): the point on the specified link to compute the Jacobian (in link local + coordinates around its center of mass). If None, it will use the CoM position (in the link frame). + q (np.array[float[N]]): joint positions of size N, where N is the number of DoFs. + dq (np.array[float[N]]): joint velocities of size N, where N is the number of DoFs. + des_ddq (np.array[float[N]]): desired joint accelerations of size N. + + Returns: + np.array[float[6,N]], np.array[float[6,6+N]]: full geometric (linear and angular) Jacobian matrix. The + number of columns depends if the base is fixed or floating. + """ + pass + + def calculate_mass_matrix(self, body_id, q): + r""" + Return the mass/inertia matrix :math:`H(q)`, which is used in the rigid-body equation of motion (EoM) in joint + space given by (see [1]): + + .. math:: \tau = H(q)\ddot{q} + C(q,\dot{q}) + + where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and + :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any + other forces acting on the system except the applied torques :math:`\tau`. + + Warnings: If the base is floating, it will return a [6+N,6+N] inertia matrix, where N is the number of actuated + joints. If the base is fixed, it will return a [N,N] inertia matrix + + Args: + body_id (int): body unique id. + q (np.array[float[N]]): joint positions of size N, where N is the total number of DoFs. + + Returns: + np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix + """ + pass + + def calculate_inverse_kinematics(self, body_id, link_id, position, orientation=None, lower_limits=None, + upper_limits=None, joint_ranges=None, rest_poses=None, joint_dampings=None, + solver=None, q_curr=None, max_iters=None, threshold=None): + r""" + Compute the FULL Inverse kinematics; it will return a position for all the actuated joints. + + "You can compute the joint angles that makes the end-effector reach a given target position in Cartesian world + space. Internally, Bullet uses an improved version of Samuel Buss Inverse Kinematics library. At the moment + only the Damped Least Squares method with or without Null Space control is exposed, with a single end-effector + target. Optionally you can also specify the target orientation of the end effector. In addition, there is an + option to use the null-space to specify joint limits and rest poses. This optional null-space support requires + all 4 lists (lower_limits, upper_limits, joint_ranges, rest_poses), otherwise regular IK will be used." [1] + + Args: + body_id (int): body unique id, as returned by `load_urdf`, etc. + link_id (int): end effector link index. + position (np.array[float[3]]): target position of the end effector (its link coordinate, not center of mass + coordinate!). By default this is in Cartesian world space, unless you provide `q_curr` joint angles. + orientation (np.array[float[4]]): target orientation in Cartesian world space, quaternion [x,y,w,z]. If not + specified, pure position IK will be used. + lower_limits (np.array[float[N]], list of N floats): lower joint limits. Optional null-space IK. + upper_limits (np.array[float[N]], list of N floats): upper joint limits. Optional null-space IK. + joint_ranges (np.array[float[N]], list of N floats): range of value of each joint. + rest_poses (np.array[float[N]], list of N floats): joint rest poses. Favor an IK solution closer to a + given rest pose. + joint_dampings (np.array[float[N]], list of N floats): joint damping factors. Allow to tune the IK solution + using joint damping factors. + solver (int): p.IK_DLS (=0) or p.IK_SDLS (=1), Damped Least Squares or Selective Damped Least Squares, as + described in the paper by Samuel Buss "Selectively Damped Least Squares for Inverse Kinematics". + q_curr (np.array[float[N]]): list of joint positions. By default PyBullet uses the joint positions of the + body. If provided, the target_position and targetOrientation is in local space! + max_iters (int): maximum number of iterations. Refine the IK solution until the distance between target + and actual end effector position is below this threshold, or the `max_iters` is reached. + threshold (float): residual threshold. Refine the IK solution until the distance between target and actual + end effector position is below this threshold, or the `max_iters` is reached. + + Returns: + np.array[float[N]]: joint positions (for each actuated joint). + """ + pass + + def calculate_inverse_dynamics(self, body_id, q, dq, des_ddq): + r""" + Starting from the specified joint positions :math:`q` and velocities :math:`\dot{q}`, it computes the joint + torques :math:`\tau` required to reach the desired joint accelerations :math:`\ddot{q}_{des}`. That is, + :math:`\tau = ID(model, q, \dot{q}, \ddot{q}_{des})`. + + Specifically, it uses the rigid-body equation of motion in joint space given by (see [1]): + + .. math:: \tau = H(q)\ddot{q} + C(q,\dot{q}) + + where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and + :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any + other forces acting on the system except the applied torques :math:`\tau`. + + Normally, a more popular form of this equation of motion (in joint space) is given by: + + .. math:: H(q) \ddot{q} + S(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F + + which is the same as the first one with :math:`C = S\dot{q} + g(q) - J^T(q) F`. However, this last formulation + is useful to understand what happens when we set some variables to 0. + Assuming that there are no forces acting on the system, and giving desired joint accelerations of 0, this + method will return :math:`\tau = S(q,\dot{q}) \dot{q} + g(q)`. If in addition joint velocities are also 0, + it will return :math:`\tau = g(q)` which can for instance be useful for gravity compensation. + + For forward dynamics, which computes the joint accelerations given the joint positions, velocities, and + torques (that is, :math:`\ddot{q} = FD(model, q, \dot{q}, \tau)`, this can be computed using + :math:`\ddot{q} = H^{-1} (\tau - C)` (see also `computeFullFD`). For more information about different + control schemes (position, force, impedance control and others), or about the formulation of the equation + of motion in task/operational space (instead of joint space), check the references [1-4]. + + Args: + body_id (int): body unique id. + q (np.array[float[N]]): joint positions + dq (np.array[float[N]]): joint velocities + des_ddq (np.array[float[N]]): desired joint accelerations + + Returns: + np.array[float[N]]: joint torques computed using the rigid-body equation of motion + + References: + - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 + - [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010 + - [3] "Springer Handbook of Robotics", Siciliano et al., 2008 + - [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma, + http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf + """ + pass + + def calculate_forward_dynamics(self, body_id, q, dq, torques): + r""" + Given the specified joint positions :math:`q` and velocities :math:`\dot{q}`, and joint torques :math:`\tau`, + it computes the joint accelerations :math:`\ddot{q}`. That is, :math:`\ddot{q} = FD(model, q, \dot{q}, \tau)`. + + Specifically, it uses the rigid-body equation of motion in joint space given by (see [1]): + + .. math:: \ddot{q} = H(q)^{-1} (\tau - C(q,\dot{q})) + + where :math:`\tau` is the vector of applied torques, :math:`H(q)` is the inertia matrix, and + :math:`C(q,\dot{q}) \dot{q}` is the vector accounting for Coriolis, centrifugal forces, gravity, and any + other forces acting on the system except the applied torques :math:`\tau`. + + Normally, a more popular form of this equation of motion (in joint space) is given by: + + .. math:: H(q) \ddot{q} + S(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F + + which is the same as the first one with :math:`C = S\dot{q} + g(q) - J^T(q) F`. However, this last formulation + is useful to understand what happens when we set some variables to 0. + Assuming that there are no forces acting on the system, and giving desired joint torques of 0, this + method will return :math:`\ddot{q} = - H(q)^{-1} (S(q,\dot{q}) \dot{q} + g(q))`. If in addition + the joint velocities are also 0, it will return :math:`\ddot{q} = - H(q)^{-1} g(q)` which are + the accelerations due to gravity. + + For inverse dynamics, which computes the joint torques given the joint positions, velocities, and + accelerations (that is, :math:`\tau = ID(model, q, \dot{q}, \ddot{q})`, this can be computed using + :math:`\tau = H(q)\ddot{q} + C(q,\dot{q})`. For more information about different + control schemes (position, force, impedance control and others), or about the formulation of the equation + of motion in task/operational space (instead of joint space), check the references [1-4]. + + Args: + body_id (int): unique body id. + q (np.array[float[N]]): joint positions + dq (np.array[float[N]]): joint velocities + torques (np.array[float[N]]): desired joint torques + + Returns: + np.array[float[N]]: joint accelerations computed using the rigid-body equation of motion + + References: + - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1 + - [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010 + - [3] "Springer Handbook of Robotics", Siciliano et al., 2008 + - [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma, + http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf + """ + pass + # Tests if __name__ == '__main__': @@ -893,8 +2259,15 @@ if __name__ == '__main__': # load robot path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/anymal/anymal.urdf' + # path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/kuka/kuka_iiwa/iiwa14.urdf' robot = sim.load_urdf(path, position=(3, -3, 2)) + print(sim.get_base_name(box)) + print(sim.get_mass(sphere)) + print(sim.get_mass(capsule)) + print(sim.get_mass(cylinder)) + print(sim.get_base_name(robot)) + # perform step for t in count(): sim.step(sleep_time=sim.dt) diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 7110fa3..15d0abf 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -18,6 +18,7 @@ References: """ from pyrobolearn.utils.data_structures.orderedset import OrderedSet +from pyrobolearn.simulators.middlewares.middleware import MiddleWare __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -199,7 +200,7 @@ class Simulator(object): self.real_time = False self.kwargs = kwargs self._num_instances = num_instances - self._middleware = middleware + self.middleware = middleware # main camera in the simulator self._camera = None @@ -246,6 +247,19 @@ class Simulator(object): """Return the simulator time step.""" return self.get_time_step() + @property + def middleware(self): + """Return the middleware.""" + return self._middleware + + @middleware.setter + def middleware(self, middleware): + """Set the middleware.""" + if middleware is not None and not isinstance(middleware, MiddleWare): + raise TypeError("Expecting the given 'middleware' to be an instance of `MiddleWare`, but got instead: " + "{}".format(middleware)) + self._middleware = middleware + ############# # Operators # ############# @@ -1695,6 +1709,26 @@ class Simulator(object): """ Set the position of the given joint(s) (using position control). + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + positions (float, np.array[float[N]]): desired position, or list of desired positions [rad] + velocities (None, float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + kps (None, float, np.array[float[N]]): position gain(s) + kds (None, float, np.array[float[N]]): velocity gain(s) + forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values. + """ + # set joint positions in the simulator + self._set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces) + + # publish the joint positions through the middleware + if self.middleware is not None: + self.middleware.set_joint_positions(body_id, joint_ids, positions, velocities, kps, kds, forces) + + def _set_joint_positions(self, body_id, joint_ids, positions, velocities=None, kps=None, kds=None, forces=None): + """ + Set the position of the given joint(s) (using position control). + Args: body_id (int): unique body id. joint_ids (int, list[int]): joint id, or list of joint ids. @@ -1710,6 +1744,39 @@ class Simulator(object): """ Get the position of the given joint(s). + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint position [rad] + if multiple joints: + np.array[float[N]]: joint positions [rad] + """ + # if a middleware is defined + if self.middleware is not None: + # get joint positions from the middleware + q = self.middleware.get_joint_positions(body_id, joint_ids) + if q is None: # if we didn't get the joint positions from the middleware, get them from the simulator + q = self._get_joint_positions(body_id, joint_ids) + else: # if we got them from the middleware, set them in the simulator + self._set_joint_positions(body_id=body_id, joint_ids=joint_ids, positions=q) + + else: + # get the joint positions from the simulator + q = self._get_joint_positions(body_id, joint_ids) + + # if the middleware is set on the teleoperation mode, publish the joint positions through the middleware + if self.middleware is not None: + self.middleware.set_joint_positions(body_id, joint_ids, q, check_teleoperate=True) + + return q + + def _get_joint_positions(self, body_id, joint_ids): + """ + Get the position of the given joint(s). + Args: body_id (int): unique body id. joint_ids (int, list[int]): joint id, or list of joint ids. @@ -1726,6 +1793,23 @@ class Simulator(object): """ Set the velocity of the given joint(s) (using velocity control). + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + max_force (None, float, np.array[float[N]]): maximum motor forces/torques + """ + # set joint velocities in the simulator + self._set_joint_velocities(body_id, joint_ids, velocities, max_force) + + # publish the joint velocities through the middleware + if self.middleware is not None: + self.middleware.set_joint_velocities(body_id, joint_ids, velocities, max_force) + + def _set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None): + """ + Set the velocity of the given joint(s) (using velocity control). + Args: body_id (int): unique body id. joint_ids (int, list[int]): joint id, or list of joint ids. @@ -1738,6 +1822,39 @@ class Simulator(object): """ Get the velocity of the given joint(s). + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + + Returns: + if 1 joint: + float: joint velocity [rad/s] + if multiple joints: + np.array[float[N]]: joint velocities [rad/s] + """ + # if a middleware is defined + if self.middleware is not None: + # get joint velocities from the middleware + dq = self.middleware.get_joint_velocities(body_id, joint_ids) + if dq is None: # if we didn't get the joint velocities from the middleware, get them from the simulator + dq = self._get_joint_velocities(body_id, joint_ids) + else: # if we got them from the middleware, set them in the simulator + self._set_joint_velocities(body_id=body_id, joint_ids=joint_ids, velocities=dq) + + else: + # get the joint velocities from the simulator + dq = self._get_joint_velocities(body_id, joint_ids) + + # if the middleware is set on the teleoperation mode, publish the joint velocities through the middleware + if self.middleware is not None: + self.middleware.set_joint_velocities(body_id, joint_ids, dq, check_teleoperate=True) + + return dq + + def _get_joint_velocities(self, body_id, joint_ids): + """ + Get the velocity of the given joint(s). + Args: body_id (int): unique body id. joint_ids (int, list[int]): joint id, or list of joint ids. @@ -1783,6 +1900,22 @@ class Simulator(object): """ Set the torque/force to the given joint(s) (using force/torque control). + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): joint id, or list of joint ids. + torques (float, list[float], np.array[float]): desired torque(s) to apply to the joint(s) [N]. + """ + # set joint torques in the simulator + self._set_joint_torques(body_id, joint_ids, torques) + + # publish the joint torques through the middleware + if self.middleware is not None: + self.middleware.set_joint_torques(body_id, joint_ids, torques) + + def _set_joint_torques(self, body_id, joint_ids, torques): + """ + Set the torque/force to the given joint(s) (using force/torque control). + Args: body_id (int): unique body id. joint_ids (int, list[int]): joint id, or list of joint ids. @@ -1794,6 +1927,39 @@ class Simulator(object): """ Get the applied torque(s) on the given joint(s). + Args: + body_id (int): unique body id. + joint_ids (int, list[int]): a joint id, or list of joint ids. + + Returns: + if 1 joint: + float: torque [Nm] + if multiple joints: + np.array[float[N]]: torques associated to the given joints [Nm] + """ + # if a middleware is defined + if self.middleware is not None: + # get joint torques from the middleware + tau = self.middleware.get_joint_torques(body_id, joint_ids) + if tau is None: # if we didn't get the joint torques from the middleware, get them from the simulator + tau = self._get_joint_torques(body_id, joint_ids) + else: # if we got them from the middleware, set them in the simulator + self._set_joint_torques(body_id=body_id, joint_ids=joint_ids, torques=tau) + + else: + # get the joint velocities from the simulator + tau = self._get_joint_torques(body_id, joint_ids) + + # if the middleware is set on the teleoperation mode, publish the joint torques through the middleware + if self.middleware is not None: + self.middleware.set_joint_torques(body_id, joint_ids, tau, check_teleoperate=True) + + return tau + + def _get_joint_torques(self, body_id, joint_ids): + """ + Get the applied torque(s) on the given joint(s). + Args: body_id (int): unique body id. joint_ids (int, list[int]): a joint id, or list of joint ids. diff --git a/pyrobolearn/utils/mesh.py b/pyrobolearn/utils/mesh.py index 1c128ff..766d465 100644 --- a/pyrobolearn/utils/mesh.py +++ b/pyrobolearn/utils/mesh.py @@ -39,7 +39,7 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -def convert_mesh(from_filename, to_filename, library='pyassimp'): +def convert_mesh(from_filename, to_filename, library='pyassimp', binary=False): """ Convert the given file containing the original mesh to the other specified format using the `pyassimp` library. @@ -47,11 +47,16 @@ def convert_mesh(from_filename, to_filename, library='pyassimp'): from_filename (str): filename of the mesh to convert. to_filename (str): filename of the converted mesh. library (str): library to use to convert the meshes. Select between 'pyassimp' and 'trimesh'. + binary (bool): if True, it will be in a binary format. This is only valid for some formats such as STL where + you have the ASCII version 'stl' and the binary version 'stlb'. """ if library == 'pyassimp': scene = pyassimp.load(from_filename) - extension = to_filename.split('.')[-1] - pyassimp.export(scene, to_filename, file_type=extension) + extension = to_filename.split('.')[-1].lower() + if binary: # for binary add 'b' as a suffix. Ex: '.stlb' + pyassimp.export(scene, to_filename, file_type=extension + 'b') + else: + pyassimp.export(scene, to_filename, file_type=extension) pyassimp.release(scene) elif library == 'trimesh': export_mesh(trimesh.load(from_filename), to_filename) diff --git a/pyrobolearn/utils/parsers/robots/data_structures.py b/pyrobolearn/utils/parsers/robots/data_structures.py index 9d85d07..848cf09 100644 --- a/pyrobolearn/utils/parsers/robots/data_structures.py +++ b/pyrobolearn/utils/parsers/robots/data_structures.py @@ -190,7 +190,7 @@ class Frame(object): using the right hand. """ # forward_axis=(1., 0., 0.), up_axis=(0., 0., 1.)): - + self.name = None self.position = position self.orientation = orientation self.dtype = dtype # world frame, body frame, joint frame, inertial frame, etc. @@ -707,7 +707,7 @@ class Floor(object): """ self.name = name self.dimensions = dimensions - self.frame = Frame(position, orientation, dtype='world') + self.frame = Frame(position=position, orientation=orientation, dtype='world') if name is not None: name = name + '_material' self.material = Material(name=name, color=color, texture=texture) @@ -867,7 +867,10 @@ class MultiBody(object): self.joints = OrderedDict() # {name: Joint} self.root = root self.materials = {} - self.frame = Frame(position, orientation, dtype='world') + self.frame = Frame(position=position, orientation=orientation, dtype='world') + + self.sensors = {} + self.actuators = {} @property def name(self): @@ -1052,6 +1055,42 @@ class MultiBody(object): # replace old joint dictionary self.joints = joints + def has_sensors(self): + """ + Return True if the multi-body data structure has some sensors. + """ + return len(self.sensors) > 0 + + def has_actuators(self): + """ + Return True if the multi-body data structure has some actuators. + """ + return len(self.actuators) > 0 + + def add_sensor(self, sensor): + """ + Add a sensor to the multi-body data structure. + + Args: + sensor (Sensor): sensor data structure. + """ + if not isinstance(sensor, Sensor): + raise TypeError("Expecting the given 'sensor' to be an instance of `Sensor`, but got instead: " + "{}".format(type(sensor))) + self.sensors[sensor.id] = sensor + + def add_actuator(self, actuator): + """ + Add an actuator to the multi-body data structure. + + Args: + actuator (Actuator): actuator data structure. + """ + if not isinstance(actuator, Actuator): + raise TypeError("Expecting the given 'actuator' to be an instance of `Actuator`, but got instead: " + "{}".format(type(actuator))) + self.actuators[actuator.id] = actuator + # alias Tree = MultiBody @@ -2831,20 +2870,39 @@ class Material(object): self._emissive = self._check_color(emissive) +class Noise(object): + """Noise distribution used in sensors and actuators.""" + pass + + +class GaussianNoise(Noise): + """Gaussian noise distribution used in sensors and actuators.""" + + def __init__(self, mean, stddev): + self.mean = mean + self.stddev = stddev + + class Sensor(object): r"""Sensor (abstract) class. """ - def __init__(self, name=None, sensors=[]): + def __init__(self, sensor_id, name=None, update_rate=None, noise=None, sensors=[]): """ Initialize the sensor. Args: + sensor_id (int): sensor unique id. name (str): name of the sensor. + update_rate (float): update rate. + noise (Noise): noise that is applied on the sensor. sensors (list[Sensor]): inner list of sensors. """ + self.id = sensor_id self.name = name + self.update_rate = update_rate + self.noise = noise self.sensors = sensors @property @@ -2859,25 +2917,255 @@ class Sensor(object): raise TypeError("Expecting the given 'name' to be a string, instead got: {}".format(type(name))) self._name = name + @property + def noise(self): + """Return the sensor noise.""" + return self._noise + + @noise.setter + def noise(self, noise): + """Set the sensor noise.""" + if noise is not None and not isinstance(noise, Noise): + raise TypeError("Expecting the given 'noise' to be an instance of `Noise` but got instead: " + "{}".format(type(noise))) + self._noise = noise + @property def num_sensors(self): """Return the number of inner sensors.""" return len(self.sensors) -class Actuator(object): # Motor - r"""Actuator/Motor (abstract) class. +class JointSensor(Sensor): + """Joint sensor + Sensor attached to a joint. """ - def __init__(self, name=None, actuators=[]): + def __init__(self, sensor_id, joint, name=None, update_rate=None, noise=None): + """ + Initialize the joint sensor. + + Args: + sensor_id (int): unique sensor id. + joint (Joint): joint to which the sensor is attached. + name (str): name of the sensor. + update_rate (float): update rate of the sensor. + noise (Noise): noise distribution that is used on the sensor. + """ + super(JointSensor, self).__init__(sensor_id, name, update_rate, noise) + self.joint = joint + + @property + def joint(self): + """Return the joint data structure.""" + return self._joint + + @joint.setter + def joint(self, joint): + """Set the joint.""" + if joint is not None and not isinstance(joint, Joint): + raise TypeError("Expecting the given 'joint' to be an instance of `Joint` but got instead: " + "{}".format(type(joint))) + self._joint = joint + + +class LinkSensor(Sensor): + """Link Sensor + + Sensor attached to a link. + """ + + def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None): + """ + Initialize the link sensor. + + Args: + sensor_id (int): unique sensor id. + link (Body): link/body to which the sensor is attached. + name (str): name of the sensor. + update_rate (float): update rate of the sensor. + noise (Noise): noise distribution that is used on the sensor. + """ + super(LinkSensor, self).__init__(sensor_id, name, update_rate, noise) + self.link = link + + @property + def link(self): + """Return the link data structure.""" + return self._link + + @link.setter + def link(self, link): + """Set the link.""" + if link is not None and not isinstance(link, Body): + raise TypeError("Expecting the given 'link' to be an instance of `Body` but got instead: " + "{}".format(type(link))) + self._link = link + + +class CameraSensor(LinkSensor): + """Camera sensor + + References: + - http://gazebosim.org/tutorials?tut=ros_gzplugins#Camera + """ + + def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None): + """ + Initialize the camera sensor. + + Args: + sensor_id (int): unique sensor id. + link (Body): link/body to which the sensor is attached. + name (str): name of the sensor. + update_rate (float): update rate of the sensor. + noise (Noise): noise distribution that is used on the sensor. + """ + super(CameraSensor, self).__init__(sensor_id, link, name, update_rate, noise) + self.frame = Frame() + self.visualize = False + + # intrinsic properties of camera + self.horizontal_fov = None + self.width = None + self.height = None + self.format = None # R8G8B8 + self.near = None + self.far = None + + self.plugin_filename = None + self.plugin_name = None + self.camera_base_topic = None # camera_base_topic + self.image_topic = None # added to the camera_base_topic + self.camera_info_topic = None # added to the camera_base_topic + self.frame_name = None # check 'name' attribute in Frame class + self.hack_baseline = None + self.distortion_k1 = None + self.distortion_k2 = None + self.distortion_k3 = None + self.distortion_t1 = None + self.distortion_t2 = None + + +class DepthCameraSensor(LinkSensor): + """Depth camera sensor + + References: + - http://gazebosim.org/tutorials?tut=ros_gzplugins#Camera + """ + + def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None): + """ + Initialize the depth camera sensor. + + Args: + sensor_id (int): unique sensor id. + link (Body): link/body to which the sensor is attached. + name (str): name of the sensor. + update_rate (float): update rate of the sensor. + noise (Noise): noise distribution that is used on the sensor. + """ + super(DepthCameraSensor, self).__init__(sensor_id, link, name, update_rate, noise) + + +class GPURay(LinkSensor): + """GPU Ray sensor + + References: + - http://gazebosim.org/tutorials?tut=ros_gzplugins#GPULaser + """ + + def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None): + """ + Initialize the GPU ray sensor. + + Args: + sensor_id (int): unique sensor id. + link (Body): link/body to which the sensor is attached. + name (str): name of the sensor. + update_rate (float): update rate of the sensor. + noise (Noise): noise distribution that is used on the sensor. + """ + super(GPURay, self).__init__(sensor_id, link, name, update_rate, noise) + + self.frame = Frame() + self.visualize = False + + # + self.horizontal = None + self.samples = None + self.scan_resolution = None + self.range_angle = None # and + + # + self.range = None # and + self.range_resolution = 0.01 + + # plugin + self.plugin_filename = None + self.plugin_name = None + self.topic = None + self.frame_name = None # Check 'name' attribute in Frame class + + +class IMUSensor(LinkSensor): + """IMU sensor. + + References: + - http://gazebosim.org/tutorials?tut=ros_gzplugins#IMUsensor(GazeboRosImuSensor) + """ + + def __init__(self, sensor_id, link, name=None, update_rate=None, noise=None): + """ + Initialize the IMU sensor. + + Args: + sensor_id (int): unique sensor id. + link (Body): link/body to which the sensor is attached. + name (str): name of the sensor. + update_rate (float): update rate of the sensor. + noise (Noise): noise distribution that is used on the sensor. + """ + super(IMUSensor, self).__init__(sensor_id, link, name, update_rate, noise) + + +class ForceTorqueSensor(JointSensor): + """Force torque sensor. + + References: + - http://gazebosim.org/tutorials?tut=force_torque_sensor&cat=sensors + - http://docs.ros.org/jade/api/gazebo_plugins/html/group__GazeboRosFTSensor.html + """ + + def __init__(self, sensor_id, joint, name=None, update_rate=None, noise=None): + """ + Initialize the F/T sensor. + + Args: + sensor_id (int): unique sensor id. + joint (Joint): joint to which the sensor is attached. + name (str): name of the sensor. + update_rate (float): update rate of the sensor. + noise (Noise): noise distribution that is used on the sensor. + """ + super(ForceTorqueSensor, self).__init__(sensor_id, joint, name, update_rate, noise) + + +class Actuator(object): # Motor + r"""Actuator/Motor (abstract) class. + """ + + def __init__(self, actuator_id, name=None, actuators=[]): """ Initialize the actuator/motor. Args: + actuator_id (int): actuator unique id. name (str): name of the actuator/motor. actuators (list[Sensor]): inner list of actuators. """ + self.id = actuator_id self.name = name self.actuators = actuators @@ -2899,6 +3187,73 @@ class Actuator(object): # Motor return len(self.actuators) +class JointActuator(Actuator): + """Joint Actuator.""" + + def __init__(self, actuator_id, joint, name=None): + """ + Initialize the joint actuator. + + Args: + actuator_id (int): unique actuator id. + joint (Joint): joint to which the actuator is attached. + name (str): name of the actuator. + """ + super(JointActuator, self).__init__(actuator_id, name) + self.joint = joint + + @property + def joint(self): + """Return the joint data structure.""" + return self._joint + + @joint.setter + def joint(self, joint): + """Set the joint.""" + if joint is not None and not isinstance(joint, Joint): + raise TypeError("Expecting the given 'joint' to be an instance of `Joint` but got instead: " + "{}".format(type(joint))) + self._joint = joint + + +class MotorJointActuator(JointActuator): + """Motor joint actuator.""" + + def __init__(self, actuator_id, joint, name=None): + """ + Initialize the joint actuator. + + Args: + actuator_id (int): unique actuator id. + joint (Joint): joint to which the actuator is attached. + name (str): name of the actuator. + """ + super(MotorJointActuator, self).__init__(actuator_id, joint, name) + + self.transmission_type = None + self.hardware_interface = None # EffortJointInterface + self.mechanical_reduction = None + + +class PositionJointActuator(MotorJointActuator): + """Position joint actuator.""" + + def __init__(self, actuator_id, joint, name=None): + """ + Initialize the position joint actuator. + + Args: + actuator_id (int): unique actuator id. + joint (Joint): joint to which the actuator is attached. + name (str): name of the actuator. + """ + super(MotorJointActuator, self).__init__(actuator_id, joint, name) + + self.p = None + self.i = None + self.d = None + + class Heightmap(object): r"""Heightmap (abstract) class. @@ -2912,3 +3267,46 @@ class Constraint(object): This allows to define a constraint. """ pass + + +class Transmission(object): + r"""Transmission interface. + + The transmission interface is used in the control loop to "describe the relationship between an actuator and a + joint. This allows one to model concepts such as gear ratios and parallel linkages. A transmission transforms + efforts/flow variables such that their product - power - remains constant. Multiple actuators may be linked to + multiple joints through complex transmission." [3] + + The control loop consists of 6 stages: + - read state from robotic hardware + - transmission: actuator to joint state + - controller manager update + + Available transmission type: + - Simple reducer (type = transmission_interface/SimpleTransmission) + - Four-bar linkage + - Differential + + References: + - [1] ROS Control: https://roscon.ros.org/2014/wp-content/uploads/2014/07/ros_control_an_overview.pdf + - [2] ros_control: http://wiki.ros.org/ros_control + - [3] URDF Transmissions: https://wiki.ros.org/urdf/XML/Transmission + """ + + def __init__(self, name, joint, transmission_type=None, actuator_name=None, hardware_interface=None): + """ + Initialize the transmission. + + Args: + name (str): name of the transmission. + joint (Joint): joint to which is attached the transmission. + transmission_type (str): transmission type; select between {'simple', 'four-bar linkage', 'differential'}. + actuator_name (str): name of the actuator. + hardware_interface (str): hardware interface. + """ + self.name = name + self.type = transmission_type + self.actuator = actuator_name + self.mechanical_reduction = None + self.joint = joint + self.hardware_interface = hardware_interface diff --git a/pyrobolearn/utils/parsers/robots/mujoco_parser.py b/pyrobolearn/utils/parsers/robots/mujoco_parser.py index 1aa555a..abf98d6 100644 --- a/pyrobolearn/utils/parsers/robots/mujoco_parser.py +++ b/pyrobolearn/utils/parsers/robots/mujoco_parser.py @@ -1286,12 +1286,13 @@ class MuJoCoParser(WorldParser): # if file does not already exists, convert it if not os.path.isfile(new_filename): - # # Arf, pyassimp export an ASCII STL, but Mujoco requires a binary STL --> use trimesh - # scene = pyassimp.load(filename) - # pyassimp.export(scene, new_filename, file_type='stl') - # pyassimp.release(scene) + # use pyassimp to + scene = pyassimp.load(filename) + pyassimp.export(scene, new_filename, file_type='stlb') + pyassimp.release(scene) - export_mesh(trimesh.load(filename), new_filename) + # + # export_mesh(trimesh.load(filename), new_filename) return new_filename return filename diff --git a/pyrobolearn/utils/parsers/robots/robot_parser.py b/pyrobolearn/utils/parsers/robots/robot_parser.py index 9a032d4..351f0bb 100644 --- a/pyrobolearn/utils/parsers/robots/robot_parser.py +++ b/pyrobolearn/utils/parsers/robots/robot_parser.py @@ -143,5 +143,6 @@ class RobotParser(object): root (ET.Element): root element in the XML file. """ xml_str = self.get_string(root) + print(xml_str) with open(filename, "w") as f: f.write(xml_str) # .encode('utf-8')) diff --git a/pyrobolearn/utils/parsers/robots/urdf_parser.py b/pyrobolearn/utils/parsers/robots/urdf_parser.py index c61e246..9f4aa9c 100644 --- a/pyrobolearn/utils/parsers/robots/urdf_parser.py +++ b/pyrobolearn/utils/parsers/robots/urdf_parser.py @@ -308,10 +308,13 @@ class URDFParser(RobotParser): tree = self.tree # create root element - root = ET.Element('robot') + attrib = {} + if tree.name is not None: + attrib['name'] = tree.name + root = ET.Element('robot', attrib=attrib) # generate material tags - for material in tree.materials: + for material in tree.materials.values(): material_tag = ET.SubElement(root, 'material', attrib={'name': material.name}) if material.color is not None: ET.SubElement(material_tag, 'color', attrib={'rgba': str(np.asarray(material.rgba))[1:-1]}) @@ -352,12 +355,15 @@ class URDFParser(RobotParser): attrib['length'] = str(geometry.size[1]) else: # mesh attrib['filename'] = geometry.filename - attrib['scale'] = str(np.asarray(geometry.size))[1:-1] + if geometry.size is not None: + size = geometry.size + size = np.array([size] * 3) if isinstance(size, (float, int)) else np.asarray(size) + attrib['scale'] = str(size)[1:-1] ET.SubElement(geometry_tag, dtype, attrib=attrib) # generate - for link in tree.bodies: + for link in tree.bodies.values(): link_tag = ET.SubElement(root, 'link', attrib={'name': link.name}) # create tag @@ -434,7 +440,7 @@ class URDFParser(RobotParser): return ET.SubElement(parent_tag, tag, attrib=kwargs) # generate - for joint in tree.joints: + for joint in tree.joints.values(): # set joint name and type joint_tag = set_name_and_type(root, 'joint', joint) @@ -443,11 +449,11 @@ class URDFParser(RobotParser): # if joint.parent is not None: - ET.SubElement(joint_tag, 'parent', attrib={'link': joint.parent}) + ET.SubElement(joint_tag, 'parent', attrib={'link': joint.parent.name}) # if joint.child is not None: - ET.SubElement(joint_tag, 'child', attrib={'link': joint.child}) + ET.SubElement(joint_tag, 'child', attrib={'link': joint.child.name}) # if joint.axis is not None: