From 605b2c81947c3b1f8c46bcd49b3f91b929b96946 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Tue, 22 Oct 2019 08:11:23 +0200 Subject: [PATCH] update ROS robot middlewares --- examples/middlewares/README.md | 2 +- .../middlewares/bullet_ros_control_gazebo.py | 8 +- .../middlewares/robot_middleware.py | 45 ++- .../simulators/middlewares/robots/franka.py | 124 +++++++- .../simulators/middlewares/robots/iiwa14.py | 283 ++++++++++++++++++ .../simulators/middlewares/robots/rrbot.py | 255 ++++++++++++++++ pyrobolearn/simulators/middlewares/ros.py | 232 ++++++++------ .../simulators/middlewares/ros_publisher.py | 56 ++-- .../simulators/middlewares/ros_subscriber.py | 57 ++-- pyrobolearn/simulators/mujoco.py | 18 +- pyrobolearn/simulators/simulator.py | 69 +++++ pyrobolearn/utils/__init__.py | 72 ++++- 12 files changed, 1056 insertions(+), 165 deletions(-) create mode 100644 pyrobolearn/simulators/middlewares/robots/iiwa14.py create mode 100644 pyrobolearn/simulators/middlewares/robots/rrbot.py diff --git a/examples/middlewares/README.md b/examples/middlewares/README.md index 76c7d70..a1019d2 100644 --- a/examples/middlewares/README.md +++ b/examples/middlewares/README.md @@ -8,7 +8,7 @@ joint trajectories to it. Here are the few examples that you can find in this folder: 1. `bullet_ros_control_gazebo.py`: After running the corresponding roslaunch file (see file documentation), you will -be able to teleoperate the manipulator (rrbot, kuka, or franka emika panda) in Gazebo by moving the same robot in +be able to teleoperate a manipulator (rrbot, kuka, or franka emika panda) in Gazebo by moving the same robot in PyBullet. The robots that are instantiated in Gazebo use position control (by using `ros_control`). A simple video demonstrating the results can be found here: https://www.youtube.com/watch?v=OPh-NCfKKK8 2. `bullet_ros_rqt.py`: this will launch RQT along PRL. diff --git a/examples/middlewares/bullet_ros_control_gazebo.py b/examples/middlewares/bullet_ros_control_gazebo.py index 8eca026..e233ddc 100644 --- a/examples/middlewares/bullet_ros_control_gazebo.py +++ b/examples/middlewares/bullet_ros_control_gazebo.py @@ -24,10 +24,10 @@ Here is a video of what it should give: https://www.youtube.com/watch?v=OPh-NCfK If you want to use 'kuka_iiwa' and 'franka', you will have to follow the same steps as above but this time by cloning: - https://github.com/IFL-CAMP/iiwa_stack -- https://github.com/mkrizmancic/franka_gazebo +- https://github.com/erdalpekel/franka_ros and https://github.com/erdalpekel/panda_simulation -Then run the corresponding roslaunch files (*_gazebo.launch) that are located in the corresponding -`pyrobolearn/robots/urdfs//` folder using the `roslaunch _gazebo.launch` command. +Then run the corresponding roslaunch files that are provided in these repositories (`simulation.launch` for the panda +robot, and `iiwa_gazebo.launch` for the kuka robot). """ import pyrobolearn as prl @@ -39,7 +39,7 @@ sim = prl.simulators.Bullet(middleware=ros) world = prl.worlds.BasicWorld(sim) # load robot -robot = world.load_robot('rrbot') # 'kuka_iiwa', 'franka' +robot = world.load_robot('rrbot') # 'kuka_iiwa', 'franka', 'rrbot' # run simulation for t in prl.count(): diff --git a/pyrobolearn/simulators/middlewares/robot_middleware.py b/pyrobolearn/simulators/middlewares/robot_middleware.py index 4d42873..dcaac87 100644 --- a/pyrobolearn/simulators/middlewares/robot_middleware.py +++ b/pyrobolearn/simulators/middlewares/robot_middleware.py @@ -229,14 +229,49 @@ class RobotMiddleware(object): """ pass - def get_jacobian(self, link_id, q=None, local_position=None): - """ - Return the jacobian. + def get_jacobian(self, 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: + link_id (int): link id. + 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). + 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. + + 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, q=None): - """ - Return the inertia matrix. + 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: + q (np.array[float[N]], None): joint positions of size N, where N is the total number of DoFs. If None, it + will get the current joint positions. + + Returns: + np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix """ pass diff --git a/pyrobolearn/simulators/middlewares/robots/franka.py b/pyrobolearn/simulators/middlewares/robots/franka.py index 745da42..15ff075 100644 --- a/pyrobolearn/simulators/middlewares/robots/franka.py +++ b/pyrobolearn/simulators/middlewares/robots/franka.py @@ -14,6 +14,9 @@ The topics for the joint states and joint commands (=joint trajectories) are: - /panda_hand_controller/command """ +import numpy as np +import rospy + # import ROS messages from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from sensor_msgs.msg import JointState @@ -59,10 +62,53 @@ class FrankaROSMiddleware(ROSRobotMiddleware): control_file (str, None): path to the YAML control file. If provided, it will be parsed. launch_file (str, None): path to the ROS launch file. If provided, it will be parsed. """ + joint_state_topic = '/joint_states' super(FrankaROSMiddleware, self).__init__(robot_id, urdf, subscribe, publish, teleoperate, command, - control_file, launch_file) + control_file, launch_file, joint_state_topics=joint_state_topic) - # update publisher and subscriber topics + # # update publisher and subscriber topics + # pub = self.publisher.create_publisher('joint_trajectory', '/panda_arm_controller/command', JointTrajectory) + # self.publisher.init_set_joint_positions(pub, msg_attribute_name='points') + # self.publisher.init_set_joint_velocities(pub, msg_attribute_name='points') + # self.publisher.init_set_joint_torques(pub, msg_attribute_name='points') + # + # self.subscriber. + # + # # joint names in the messages + # self.msg_joint_names = ['panda_finger_joint1', 'panda_finger_joint2'] + \ + # ['panda_joint' + str(i+1) for i in range(7)] + # + # # set joint names and trajectory point in message + # pub.msg.joint_names = self.msg_joint_names[2:] + self.msg_joint_names[:2] + # pub.msg.points = [JointTrajectoryPoint()] + + # joint names in the messages + self.msg_joint_names = ['panda_finger_joint1', 'panda_finger_joint2'] + \ + ['panda_joint' + str(i+1) for i in range(7)] + + # joint trajectory point instance + self.arm_point = JointTrajectoryPoint() + self.arm_point.positions = np.zeros(7) + # self.arm_point.velocities = 0.1 * np.ones(7) + # self.arm_point.effort = 0.1 * np.ones(7) + self.hand_point = JointTrajectoryPoint() + self.hand_point.positions = np.zeros(2) + # self.hand_point.velocities = 0.1 * np.ones(2) + # self.hand_point.effort = 0.1 * np.ones(2) + + # update publisher + arm_topic = '/panda_arm_controller/command' + self.arm_publisher = self.publisher.create_publisher(name='panda_arm_trajectory', topic=arm_topic, + msg_class=JointTrajectory) + hand_topic = '/panda_hand_controller/command' + self.hand_publisher = self.publisher.create_publisher(name='panda_hand_trajectory', topic=hand_topic, + msg_class=JointTrajectory) + + # set joint names and trajectory point in message + self.arm_publisher.msg.joint_names = self.msg_joint_names[2:] + self.arm_publisher.msg.points = [self.arm_point] + self.hand_publisher.msg.joint_names = self.msg_joint_names[:2] + self.hand_publisher.msg.points = [self.hand_point] def get_joint_positions(self, joint_ids=None): """ @@ -77,7 +123,9 @@ class FrankaROSMiddleware(ROSRobotMiddleware): if multiple joints: np.array[float[N]]: joint positions [rad] """ - pass + if self.is_subscribing: + q_indices = None if joint_ids is None else self.q_indices[joint_ids] + return self.subscriber.get_joint_positions(q_indices) def set_joint_positions(self, positions, joint_ids=None, velocities=None, kps=None, kds=None, forces=None): """ @@ -91,7 +139,37 @@ class FrankaROSMiddleware(ROSRobotMiddleware): 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 + if self.is_publishing: + q = self.subscriber.get_joint_positions() + dq = self.subscriber.get_joint_velocities() + tau = self.subscriber.get_joint_torques() + + if len(q) > 0: + q_indices = None if joint_ids is None else self.q_indices[joint_ids] + if q_indices is not None: + q[q_indices] = positions + if velocities is not None: + dq[q_indices] = velocities + + self.arm_point.positions = q[:7] + # self.arm_point.velocities = dq[:7] + # self.arm_point.effort = tau[:7] + + self.hand_point.positions = q[7:] + # self.hand_point.velocities = dq[7:] + # self.hand_point.effort = tau[7:] + + # set time duration + self.arm_point.time_from_start.secs = 0 + self.arm_point.time_from_start.nsecs = 200000000 + self.hand_point.time_from_start.secs = 0 + self.hand_point.time_from_start.nsecs = 200000000 + + # set message and publish it + self.arm_publisher.msg.points = [self.arm_point] + self.hand_publisher.msg.points = [self.hand_point] + self.arm_publisher.publish() + self.hand_publisher.publish() def get_joint_velocities(self, joint_ids=None): """ @@ -193,25 +271,49 @@ class FrankaROSMiddleware(ROSRobotMiddleware): """ pass - def get_jacobian(self, link_id, q=None, local_position=None): - """ - Return the jacobian. + def get_jacobian(self, 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: 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. 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). + 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. + + 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, q=None): - """ - Return the inertia matrix. + 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: q (np.array[float[N]], None): joint positions of size N, where N is the total number of DoFs. If None, it will get the current joint positions. + + Returns: + np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix """ pass diff --git a/pyrobolearn/simulators/middlewares/robots/iiwa14.py b/pyrobolearn/simulators/middlewares/robots/iiwa14.py new file mode 100644 index 0000000..bde2a9f --- /dev/null +++ b/pyrobolearn/simulators/middlewares/robots/iiwa14.py @@ -0,0 +1,283 @@ +# -*- coding: utf-8 -*- +#!/usr/bin/env python +"""Define the Kuka IIWA ROS Robot middleware API. + +This is robot middleware interface between the Kuka IIWA robot and ROS. This file should be modified by the user!! +Currently, we use the setup provided in: https://github.com/IFL-CAMP/iiwa_stack +by launching `iiwa_gazebo.launch` and keeping the `trajectory` argument set to be true. + +The topics for the joint states (sensor_msgs.JointState) and joint commands (std_msgs.Float64) are: +- /iiwa/joint_states +- /iiwa/PositionJointInterface_trajectory_controller/command +""" + +import rospy +import numpy as np + +# import ROS messages +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from sensor_msgs.msg import JointState + +from pyrobolearn.simulators.middlewares.ros import ROSRobotMiddleware + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class KukaIIWAROSMiddleware(ROSRobotMiddleware): + r"""Robot middleware interface. + + The robot middleware interface is an interface between a particular robot and the middleware. The middleware + possesses a list of Robot middleware interfaces (one for each robot). + + Notably, the robot middleware has a unique id, has a list of publishers and subscribers associated with the given + robot. + """ + + def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True, + control_file=None, launch_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`. + command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will + subscribe/publish to some (joint) states. + control_file (str, None): path to the YAML control file. If provided, it will be parsed. + launch_file (str, None): path to the ROS launch file. If provided, it will be parsed. + """ + joint_state_topic = '/iiwa/joint_states' + super(KukaIIWAROSMiddleware, self).__init__(robot_id, urdf, subscribe, publish, teleoperate, command, + control_file, launch_file, joint_state_topics=joint_state_topic) + + # joint names in the messages + self.msg_joint_names = ['iiwa_joint_' + str(i + 1) for i in range(7)] + + # joint trajectory point instance + self.joint_trajectory_point = JointTrajectoryPoint() + self.joint_trajectory_point.positions = np.zeros(self.tree.num_actuated_joints) + self.joint_trajectory_point.velocities = 1. * np.ones(self.tree.num_actuated_joints) + self.joint_trajectory_point.effort = 1. * np.ones(self.tree.num_actuated_joints) + + # update publisher + joint_trajectory_topic = '/iiwa/PositionJointInterface_trajectory_controller/command' + self.joint_trajectory_publisher = self.publisher.create_publisher(name='joint_trajectory', + topic=joint_trajectory_topic, + msg_class=JointTrajectory) + + # set joint names and trajectory point in message + self.joint_trajectory_publisher.msg.joint_names = self.msg_joint_names + self.joint_trajectory_publisher.msg.points = [self.joint_trajectory_point] + + def get_joint_positions(self, joint_ids=None): + """ + Get the position of the given joint(s). + + Args: + joint_ids (int, list[int], None): 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 self.is_subscribing: + q_indices = None if joint_ids is None else self.q_indices[joint_ids] + return self.subscriber.get_joint_positions(q_indices) + + def set_joint_positions(self, positions, joint_ids=None, velocities=None, kps=None, kds=None, forces=None): + """ + Set the position of the given joint(s) (using position control). + + Args: + positions (float, np.array[float[N]]): desired position, or list of desired positions [rad] + joint_ids (int, list[int], None): joint id, or list of joint ids. + 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. + """ + if self.is_publishing: + q = self.subscriber.get_joint_positions() + dq = self.subscriber.get_joint_velocities() + tau = self.subscriber.get_joint_torques() + + if len(q) > 0: + q_indices = None if joint_ids is None else self.q_indices[joint_ids] + if q_indices is not None: + q[q_indices] = positions + if velocities is not None: + dq[q_indices] = velocities + + self.joint_trajectory_point.positions = q + # self.joint_trajectory_point.velocities = dq + # self.joint_trajectory_point.effort = tau + + # set time duration + self.joint_trajectory_point.time_from_start.secs = 0 + self.joint_trajectory_point.time_from_start.nsecs = 200000000 + + # set message and publish it + self.joint_trajectory_publisher.msg.points = [self.joint_trajectory_point] + self.joint_trajectory_publisher.publish() + + def get_joint_velocities(self, joint_ids=None): + """ + Get the velocity of the given joint(s). + + Args: + joint_ids (int, list[int], None): 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, velocities, joint_ids=None, max_force=None): + """ + Set the velocity of the given joint(s) (using velocity control). + + Args: + velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + joint_ids (int, list[int], None): joint id, or list of joint ids. + max_force (None, float, np.array[float[N]]): maximum motor forces/torques. + """ + pass + + def get_joint_torques(self, joint_ids=None): + """ + 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], None): 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, torques, joint_ids=None): + """ + Set the torque/force to the given joint(s) (using force/torque control). + + Args: + torques (float, list[float]): desired torque(s) to apply to the joint(s) [N]. + joint_ids (int, list[int], None): joint id, or list of joint ids. + """ + pass + + def has_sensor(self, name): + """ + Check if the given robot middleware has the specified sensor. + + Args: + name (str): name of the sensor. + + Returns: + bool: True if the robot middleware has the sensor. + """ + pass + + def get_sensor_values(self, name): + """ + Get the sensor values associated with the given sensor name. + + Args: + name (str): unique name of the sensor. + + Returns: + object, np.array, float, int: sensor values. + """ + pass + + def get_pid(self, joint_ids): + """ + Get the PID coefficients associated to the given joint ids. + + Args: + joint_ids (list[int]): list of unique joint ids. + + Returns: + list[np.array[float[3]]]: list of PID coefficients for each joint. + """ + pass + + def set_pid(self, joint_ids, pid): + """ + Set the given PID coefficients to the given joint ids. + + Args: + joint_ids (list[int]): list of unique joint ids. + pid (list[np.array[float[3]]]): list of PID coefficients for each joint. If one of the value is -1, it + will left untouched the associated PID value to the previous one. + """ + pass + + def get_jacobian(self, 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: + link_id (int): link id. + 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). + 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. + + 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, q=None): + 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: + q (np.array[float[N]], None): joint positions of size N, where N is the total number of DoFs. If None, it + will get the current joint positions. + + Returns: + np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix + """ + pass diff --git a/pyrobolearn/simulators/middlewares/robots/rrbot.py b/pyrobolearn/simulators/middlewares/robots/rrbot.py new file mode 100644 index 0000000..471ec51 --- /dev/null +++ b/pyrobolearn/simulators/middlewares/robots/rrbot.py @@ -0,0 +1,255 @@ +# -*- coding: utf-8 -*- +#!/usr/bin/env python +"""Define the RRBot ROS Robot middleware API. + +This is robot middleware interface between the RRBot robot and ROS. This file should be modified by the user!! +Currently, we use the setup provided in: https://github.com/ros-simulation/gazebo_ros_demos +by launching `rrbot_world.launch` and `rrbot_control.launch`. + +The topics for the joint states (sensor_msgs.JointState) and joint commands (std_msgs.Float64) are: +- /rrbot/joint_states +- /rrbot/joint1_position_controller/command +- /rrbot/joint2_position_controller/command +""" + +# import ROS messages +import std_msgs.msg as std_msg +from sensor_msgs.msg import JointState + +from pyrobolearn.simulators.middlewares.ros import ROSRobotMiddleware + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RRBotROSMiddleware(ROSRobotMiddleware): + r"""Robot middleware interface. + + The robot middleware interface is an interface between a particular robot and the middleware. The middleware + possesses a list of Robot middleware interfaces (one for each robot). + + Notably, the robot middleware has a unique id, has a list of publishers and subscribers associated with the given + robot. + """ + + def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True, + control_file=None, launch_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`. + command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will + subscribe/publish to some (joint) states. + control_file (str, None): path to the YAML control file. If provided, it will be parsed. + launch_file (str, None): path to the ROS launch file. If provided, it will be parsed. + """ + print("Creating RRBot Publisher.") + super(RRBotROSMiddleware, self).__init__(robot_id, urdf, subscribe, publish, teleoperate, command, + control_file, launch_file) + + # create publishing topics + topics = [] + count = 0 + for joint in self.tree.joints.values(): + if joint.dtype != 'fixed': + topic = '/rrbot/joint' + str(count + 1) + '_position_controller/command' + topics.append(topic) + count += 1 + print("Publishing Topics: ", topics) + publisher = self.publisher.create_publisher(name='qpos', topic=topics, msg_class=std_msg.Float64) + self.publisher.init_set_joint_positions(publisher=publisher, msg_attribute_name='data') + + def get_joint_positions(self, joint_ids=None): + """ + Get the position of the given joint(s). + + Args: + joint_ids (int, list[int], None): 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 self.is_subscribing: + q_indices = None if joint_ids is None else self.q_indices[joint_ids] + return self.subscriber.get_joint_positions(q_indices) + + def set_joint_positions(self, positions, joint_ids=None, velocities=None, kps=None, kds=None, forces=None): + """ + Set the position of the given joint(s) (using position control). + + Args: + positions (float, np.array[float[N]]): desired position, or list of desired positions [rad] + joint_ids (int, list[int], None): joint id, or list of joint ids. + 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. + """ + if self.is_publishing: + q_indices = None if joint_ids is None else self.q_indices[joint_ids] + self.publisher.set_joint_positions(positions, q_indices=q_indices) + self.publisher.publish('qpos') + + def get_joint_velocities(self, joint_ids=None): + """ + Get the velocity of the given joint(s). + + Args: + joint_ids (int, list[int], None): 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, velocities, joint_ids=None, max_force=None): + """ + Set the velocity of the given joint(s) (using velocity control). + + Args: + velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s] + joint_ids (int, list[int], None): joint id, or list of joint ids. + max_force (None, float, np.array[float[N]]): maximum motor forces/torques. + """ + pass + + def get_joint_torques(self, joint_ids=None): + """ + 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], None): 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, torques, joint_ids=None): + """ + Set the torque/force to the given joint(s) (using force/torque control). + + Args: + torques (float, list[float]): desired torque(s) to apply to the joint(s) [N]. + joint_ids (int, list[int], None): joint id, or list of joint ids. + """ + pass + + def has_sensor(self, name): + """ + Check if the given robot middleware has the specified sensor. + + Args: + name (str): name of the sensor. + + Returns: + bool: True if the robot middleware has the sensor. + """ + pass + + def get_sensor_values(self, name): + """ + Get the sensor values associated with the given sensor name. + + Args: + name (str): unique name of the sensor. + + Returns: + object, np.array, float, int: sensor values. + """ + pass + + def get_pid(self, joint_ids): + """ + Get the PID coefficients associated to the given joint ids. + + Args: + joint_ids (list[int]): list of unique joint ids. + + Returns: + list[np.array[float[3]]]: list of PID coefficients for each joint. + """ + pass + + def set_pid(self, joint_ids, pid): + """ + Set the given PID coefficients to the given joint ids. + + Args: + joint_ids (list[int]): list of unique joint ids. + pid (list[np.array[float[3]]]): list of PID coefficients for each joint. If one of the value is -1, it + will left untouched the associated PID value to the previous one. + """ + pass + + def get_jacobian(self, 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: + link_id (int): link id. + 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). + 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. + + 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, q=None): + 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: + q (np.array[float[N]], None): joint positions of size N, where N is the total number of DoFs. If None, it + will get the current joint positions. + + 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 803d14c..0cd2b3d 100644 --- a/pyrobolearn/simulators/middlewares/ros.py +++ b/pyrobolearn/simulators/middlewares/ros.py @@ -176,7 +176,7 @@ class ROSRobotMiddleware(RobotMiddleware): """ def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True, - control_file=None, launch_file=None): + control_file=None, launch_file=None, joint_state_topics=None, joint_state_msg_class=None): """ Initialize the robot middleware interface. @@ -192,6 +192,10 @@ class ROSRobotMiddleware(RobotMiddleware): subscribe/publish to some (joint) states. control_file (str, None): path to the YAML control file. If provided, it will be parsed. launch_file (str, None): path to the ROS launch file. If provided, it will be parsed. + joint_state_topics (str, list[str]): joint state topic(s). If not provided the joint state topic will be + set to '//joint_states'. + joint_state_msg_class (class): message serialization class used for the provided joint state topic. By + default, it will be set to 'sensor_msg.JointState'. """ super(ROSRobotMiddleware, self).__init__(robot_id, urdf, subscribe, publish, teleoperate, command, control_file) self.launch_file = launch_file @@ -200,7 +204,7 @@ class ROSRobotMiddleware(RobotMiddleware): # get path to the URDF folder path = os.path.abspath(urdf) # /path/to/pyrobolearn/robots/urdfs//robot.urdf - dirname = str(os.path.dirname(path)) # /path/to/pyrobolearn/robots/urdfs// + # dirname = str(os.path.dirname(path)) # /path/to/pyrobolearn/robots/urdfs// basename = str(os.path.basename(path).split('.')[-2]) # robot name without extension # parse URDF to get joint names, q indices, etc. @@ -215,15 +219,16 @@ class ROSRobotMiddleware(RobotMiddleware): count = 0 for i, joint in enumerate(tree.joints.values()): if joint.dtype != 'fixed': - print("Adding joint {} with type={}".format(joint.name, joint.dtype)) + print("Adding joint {} with type={}, q_idx={}".format(joint.name, joint.dtype, i)) self.q_indices[i] = count self.joint_names.append(joint.name) count += 1 # subscriber and publisher associated with the given robot - if self.is_subscribing: - print("Creating Robot Subscriber") - self.subscriber = RobotSubscriber(name=basename) + # if self.is_subscribing: + print("Creating Robot Subscriber") + self.subscriber = RobotSubscriber(name=basename, joint_state_topics=joint_state_topics, + joint_state_msg_class=joint_state_msg_class) if self.is_publishing: print("Creating Robot Publisher") self.publisher = RobotPublisher(name=basename) @@ -255,7 +260,7 @@ class ROSRobotMiddleware(RobotMiddleware): Returns: PublisherData: the publisher data holder. """ - return self.publisher.create_publisher(name=name, topic=topic, data_class=msg_class, queue_size=queue_size) + return self.publisher.create_publisher(name=name, topic=topic, msg_class=msg_class, queue_size=queue_size) def create_subscriber(self, name, topic, msg_class): """ @@ -271,7 +276,7 @@ class ROSRobotMiddleware(RobotMiddleware): Returns: SubscriberData: the subscriber data holder. """ - return self.subscriber.create_subscriber(name=name, topic=topic, data_class=msg_class) + return self.subscriber.create_subscriber(name=name, topic=topic, msg_class=msg_class) def change_topic(self, old_topic, new_topic, new_msg=None, queue_size=None): """ @@ -295,6 +300,67 @@ class ROSRobotMiddleware(RobotMiddleware): if self.subscriber.has_subscriber(old_topic): self.subscriber.change_topic(old_topic=old_topic, new_topic=new_topic, new_msg=new_msg) + +class DefaultROSRobotMiddleware(ROSRobotMiddleware): + r"""Default ROS robot middleware interface. + + This is the default ROS robot middleware interface which can be created when no specific interfaces are provided + by the user. Specific interfaces can be found in the `robots` folder. + + Here are the possible combinations between the different values for subscribe (S), publish (P), teleoperate (T), + and command (C): + + - S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods. + - S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods. + - S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The received + commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to topics that + publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory commands, or joint + states when teleoperating the robot in the simulator? This C value allows to specify which one we are interested + in. + - S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also + publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator. + - S=0, P=0, T=1/0: doesn't do anything. + - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be + sent/received by calling the appropriate getter/setter methods. + - S=1, P=1, T=1: not allowed. + """ + + def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True, + control_file=None, launch_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`. + command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will + subscribe/publish to some (joint) states. + control_file (str, None): path to the YAML control file. + launch_file (str, None): path to the ROS launch file. If provided, it will be parsed. + """ + # set variables + super(DefaultROSRobotMiddleware, self).__init__(robot_id, urdf=urdf, subscribe=subscribe, publish=publish, + teleoperate=teleoperate, command=command, + control_file=control_file, launch_file=launch_file) + + if self.is_publishing: + topics = [] + count = 0 + for i, joint in enumerate(self.tree.joints.values()): + if joint.dtype != 'fixed': + topic = '/' + self.tree.name + '/joint' + str(count+1) + '_position_controller/command' + topics.append(topic) + print("Publishing Topics: ", topics) + publisher = self.publisher.create_publisher(name='qpos', topic=topics, msg_class=std_msg.Float64) + self.publisher.init_set_joint_positions(publisher=publisher, msg_attribute_name='data') + + # sensors + def get_joint_positions(self, joint_ids=None): """ Get the position of the given joint(s). @@ -441,98 +507,54 @@ class ROSRobotMiddleware(RobotMiddleware): """ pass - def get_jacobian(self, link_id, q=None, local_position=None): - """ - Return the full geometric jacobian. + def get_jacobian(self, 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: 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. 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). + 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. + + 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, q=None): - """ - Return the inertia matrix. + 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: q (np.array[float[N]], None): joint positions of size N, where N is the total number of DoFs. If None, it will get the current joint positions. + + Returns: + np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix """ pass -class DefaultROSRobotMiddleware(ROSRobotMiddleware): - r"""Default ROS robot middleware interface. - - This is the default ROS robot middleware interface which can be created when no specific interfaces are provided - by the user. Specific interfaces can be found in the `robots` folder. - - Here are the possible combinations between the different values for subscribe (S), publish (P), teleoperate (T), - and command (C): - - - S=1, P=0, T=0: subscribes to the topics, and get the messages when calling the corresponding getter methods. - - S=0, P=1, T=0: publishes the various messages to the topics when calling the corresponding setter methods. - - S=1, P=0, T=1, C=0/1: get messages by subscribing to the topics that publish some commands/states. The received - commands/states are then set in the simulator. Depending on the value of `C`, it will subscribe to topics that - publish some commands (C=1) or states (C=0). Example: should we subscribe to joint trajectory commands, or joint - states when teleoperating the robot in the simulator? This C value allows to specify which one we are interested - in. - - S=0, P=1, T=1: when calling the getters methods such as joint positions, velocities, and others, it also - publishes the joint states. This is useful if we are moving/teleoperating the robot in the simulator. - - S=0, P=0, T=1/0: doesn't do anything. - - S=1, P=1, T=0: subscribes to some topics and publish messages to other topics. The messages are can be - sent/received by calling the appropriate getter/setter methods. - - S=1, P=1, T=1: not allowed. - """ - - def __init__(self, robot_id, urdf=None, subscribe=False, publish=False, teleoperate=False, command=True, - 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`. - command (bool): if True, it will subscribe/publish to some (joint) commands. If False, it will - subscribe/publish to some (joint) states. - control_file (str, None): path to the YAML control file. - """ - # set variables - super(DefaultROSRobotMiddleware, self).__init__(robot_id, urdf, subscribe, publish, teleoperate, command, - control_file) - - if self.is_publishing: - urdf_parser = URDFParser() - tree = urdf_parser.parse(urdf) - print("ROSRobotMiddleware - publisher - name: ", tree.name) - print("Num joints: ", tree.num_joints) - print("Num actuated joints: ", tree.num_actuated_joints) - self.q_indices = np.zeros(tree.num_joints, dtype=int) - topics = [] - count = 0 - for i, joint in enumerate(tree.joints.values()): - if joint.dtype != 'fixed': - print("Adding joint {} with type={}".format(joint.name, joint.dtype)) - topic = '/' + tree.name + '/joint' + str(count+1) + '_position_controller/command' - self.q_indices[i] = count - count += 1 - topics.append(topic) - print("Publishing Topics: ", topics) - publisher = self.publisher.create_publisher(name='qpos', topic=topics, data_class=std_msg.Float64) - self.publisher.init_set_joint_positions(publisher=publisher, msg_attribute_name='data') - - # sensors - - class ROS(Middleware): r"""ROS Interface middleware @@ -613,6 +635,8 @@ class ROS(Middleware): self.processes = {} # {Node: process} + rospy.init_node(self.__class__.__name__, anonymous=True) + ########### # Methods # ########### @@ -1337,17 +1361,43 @@ class ROS(Middleware): """ # check URDF path; check if it is a valid robot directory. If not a robot, just skip path = os.path.dirname(os.path.abspath(urdf)) # /path/to/pyrobolearn/robots/urdfs// - robot_path = '/'.join(path.split('/')[-4:-1]) + # robot_path = '/'.join(path.split('/')[-4:-1]) if 'pyrobolearn/robots/urdfs' in path: id_ = self.count_id self.count_id += 1 # check if specific robot middleware exists in `robots` folder, and if so load it + dirname = os.path.dirname(os.path.abspath(__file__)) + robot_name = str(os.path.basename(os.path.abspath(urdf)).split('.')[-2]) # name without extension (.urdf) + if os.path.exists(dirname + '/robots/' + robot_name + '.py'): + # import module + module = importlib.import_module('pyrobolearn.simulators.middlewares.robots.' + robot_name) + + def predicate(cls): + return inspect.isclass(cls) and issubclass(cls, ROSRobotMiddleware) and cls != ROSRobotMiddleware + + # get classes inside modules that are a subclass of ROSRobotMiddleware + classes = dict(inspect.getmembers(module, predicate)) + cls = DefaultROSRobotMiddleware + + # get first specific ROS robot middleware class if present + for key in classes: + cls = classes[key] + if cls != ROSRobotMiddleware: + break + + print("Creating specific robot middleware: ", cls.__name__) + + robot = cls(id_, urdf=urdf, subscribe=self.is_subscribing, publish=self.is_publishing, + teleoperate=self.is_teleoperating, command=self.is_commanding, control_file=None, + launch_file=None) # otherwise, create default robot middleware - robot = DefaultROSRobotMiddleware(id_, urdf=urdf, subscribe=self.is_subscribing, - publish=self.is_publishing, teleoperate=self.is_teleoperating, - command=self.is_commanding, control_file=None) + else: + print("Creating default robot middleware") + robot = DefaultROSRobotMiddleware(id_, urdf=urdf, subscribe=self.is_subscribing, + publish=self.is_publishing, teleoperate=self.is_teleoperating, + command=self.is_commanding, control_file=None) self._robots[id_] = robot return id_ @@ -1489,7 +1539,9 @@ class ROS(Middleware): 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 + robot = self._robots.get(body_id) + if robot is not None: + return robot.get_jacobian(link_id, local_position=local_position, q=q) def get_inertia_matrix(self, body_id, q): r""" @@ -1512,7 +1564,9 @@ class ROS(Middleware): Returns: np.array[float[N,N]], np.array[float[6+N,6+N]]: inertia matrix """ - pass + robot = self._robots.get(body_id) + if robot is not None: + return robot.get_inertia_matrix(q) def has_sensor(self, body_id, name): """ diff --git a/pyrobolearn/simulators/middlewares/ros_publisher.py b/pyrobolearn/simulators/middlewares/ros_publisher.py index 9ec485c..7c51b5e 100644 --- a/pyrobolearn/simulators/middlewares/ros_publisher.py +++ b/pyrobolearn/simulators/middlewares/ros_publisher.py @@ -35,35 +35,38 @@ class PublisherData(object): `pub.msg.data`). """ - def __init__(self, topic, data_class, queue_size=10): + def __init__(self, topic, msg_class, queue_size=10): """ Initialize the PublisherData that publishes the given message data. Args: topic (str, list[str]): topic name(s). If multiple topics are given, it will group them. Note that you can only group topics that use the same message class. - data_class (class): message class for serialization. + msg_class (class): message class for serialization. queue_size (int): The queue size used for asynchronously publishing messages from different threads. A size of zero means an infinite queue, which can be dangerous. When None is passed all publishing will happen synchronously and a warning message will be printed. """ - # self.__dict__['publisher'] = rospy.Publisher(topic, data_class, queue_size=queue_size) + # self.__dict__['publisher'] = rospy.Publisher(topic, msg_class, queue_size=queue_size) # # set the message attributes to be part of this class attributes - # 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__['msg'] = data_class() + # self.__dict__['attributes'] = [attr for attr in [attr for attr in dir(msg_class) if not attr.startswith('_')] + # if not callable(getattr(msg_class, attr))] + # self.__dict__['msg'] = msg_class() self.topic = topic self.queue_size = queue_size - self.msg_class = data_class - if isinstance(topic, collections.Iterable): - self.is_group = True - self.publisher = [rospy.Publisher(t, data_class, queue_size=queue_size) for t in topic] - self.msg = [data_class() for _ in topic] - else: + self.msg_class = msg_class + if isinstance(topic, str): self.is_group = False - self.publisher = rospy.Publisher(topic, data_class, queue_size=queue_size) - self.msg = data_class() + self.publisher = rospy.Publisher(topic, msg_class, queue_size=queue_size) + self.msg = msg_class() + elif isinstance(topic, collections.Iterable): + self.is_group = True + self.publisher = [rospy.Publisher(t, msg_class, queue_size=queue_size) for t in topic] + self.msg = [msg_class() for _ in topic] + else: + raise TypeError("Expecting the given 'topic' to a str, or a list of str, but instead got: " + "{}".format(type(topic))) def publish(self, data=None, indices=None, replace=True): """ @@ -125,6 +128,7 @@ class PublisherData(object): indices (None, list[int], int): if multiple topics are defined for this class, you can specify which index to use. """ + # TODO: use `rsetattr` (which is implemented in pyrobolearn/utils/__init__.py) if self.is_group: if indices is None: # set every message attribute if isinstance(values, collections.Iterable): @@ -159,6 +163,7 @@ class PublisherData(object): Returns: object: message attribute value(s). """ + # TODO: use `rgetattr` (which is implemented in pyrobolearn/utils/__init__.py) if self.is_group: if indices is None: return [getattr(msg, key) for msg in self.msg] @@ -209,17 +214,20 @@ class Publisher(object): """ # 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)) + # # try: + # if publisher_id is None: + # rospy.init_node(self.__class__.__name__, anonymous=True) + # else: + # rospy.init_node(self.__class__.__name__ + str(publisher_id)) + # # except rospy.exceptions.ROSException: # node already initialized + # # pass # all publishers {publisher name: PublisherData} self.publishers = dict() # all topics {topic: publisher name} self.topics_to_publisher_name = dict() - def create_publisher(self, name, topic, data_class, queue_size=10): + def create_publisher(self, name, topic, msg_class, queue_size=10): """ Create a publisher to the specific topic. If the publisher already exists, it unregister the previous one and replace it by the new one. @@ -228,7 +236,7 @@ class Publisher(object): name (str): unique name of the publisher. The name must be unique. You will be able to access to this publisher using its name. topic (str, list[str]): name of the topic(s). - data_class (object): data type class to use for messages + msg_class (object): data type class to use for messages queue_size (int): The queue size used for asynchronously publishing messages from different threads. A size of zero means an infinite queue, which can be dangerous. When None is passed all publishing will happen synchronously and a warning message will be printed. @@ -240,9 +248,13 @@ class Publisher(object): self.remove_publisher(name) # create new publisher - publisher = PublisherData(topic, data_class, queue_size=queue_size) + publisher = PublisherData(topic=topic, msg_class=msg_class, queue_size=queue_size) self.publishers[name] = publisher - self.topics_to_publisher_name[topic] = name + if isinstance(topic, collections.Iterable): + for t in topic: + self.topics_to_publisher_name[t] = name + else: + self.topics_to_publisher_name[topic] = name return publisher diff --git a/pyrobolearn/simulators/middlewares/ros_subscriber.py b/pyrobolearn/simulators/middlewares/ros_subscriber.py index 6af7f98..a839e4c 100644 --- a/pyrobolearn/simulators/middlewares/ros_subscriber.py +++ b/pyrobolearn/simulators/middlewares/ros_subscriber.py @@ -31,32 +31,35 @@ class SubscriberData(object): from this class. """ - def __init__(self, topic, data_class): + def __init__(self, topic, msg_class): """ Initialize the SubscriberData that subscribes to the given topic. Args: topic (str, list[str]): topic name(s). If multiple topics are given, it will group them. Note that you can only group topics that use the same message class. - data_class (class): message class for serialization. + msg_class (class): message class for serialization. """ - # self.subscriber = rospy.Subscriber(topic, data_class, callback=self.callback) + # self.subscriber = rospy.Subscriber(topic, msg_class, callback=self.callback) # # set the message attributes to be part of this class attributes - # 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() + # self.attributes = set([attr for attr in [attr for attr in dir(msg_class) if not attr.startswith('_')] + # if not callable(getattr(msg_class, attr))]) + # self.subscriber_data = msg_class() self.topic = topic - self.msg_class = data_class - if isinstance(topic, collections.Iterable): - self.is_group = True - self.subscriber = [rospy.Subscriber(t, data_class, callback=self.callback, callback_args=idx) - for idx, t in enumerate(topic)] - self.msg = [data_class() for _ in topic] - else: + self.msg_class = msg_class + if isinstance(topic, str): self.is_group = False - self.subscriber = rospy.Subscriber(topic, data_class, callback=self.callback) - self.msg = data_class() + self.subscriber = rospy.Subscriber(topic, msg_class, callback=self.callback) + self.msg = msg_class() + elif isinstance(topic, collections.Iterable): + self.is_group = True + self.subscriber = [rospy.Subscriber(t, msg_class, callback=self.callback, callback_args=idx) + for idx, t in enumerate(topic)] + self.msg = [msg_class() for _ in topic] + else: + raise TypeError("Expecting the given 'topic' to a str, or a list of str, but instead got: " + "{}".format(type(topic))) def callback(self, data, idx=None): """ @@ -134,17 +137,20 @@ class Subscriber(object): 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)) + # try: + # if subscriber_id is None: + # rospy.init_node(self.__class__.__name__, anonymous=True) + # else: + # rospy.init_node(self.__class__.__name__ + str(subscriber_id)) + # except rospy.exceptions.ROSException: # node already initialized + # pass # all subscribers {subscriber name: SubscriberData} self.subscribers = dict() # all topics {topic: subscriber name} self.topics_to_subscriber_name = dict() - def create_subscriber(self, name, topic, data_class): + def create_subscriber(self, name, topic, msg_class): """ Create a subscriber to the specific topic. If the subscriber already exists, it unregister the previous one and replace it by the new one. @@ -153,7 +159,7 @@ class Subscriber(object): name (str): unique name of the subscriber. The name must be unique. You will be able to access to this subscriber using its name. topic (str, list[str]): name of the topic(s). - data_class (object): data type class to use for messages + msg_class (object): data type class to use for messages Returns: SubscriberData: the subscriber data holder. @@ -162,9 +168,14 @@ class Subscriber(object): self.remove_subscriber(name) # create new subscriber - subscriber = SubscriberData(topic, data_class) + subscriber = SubscriberData(topic, msg_class) self.subscribers[name] = subscriber - self.topics_to_subscriber_name[topic] = name + if isinstance(topic, collections.Iterable): + for t in topic: + self.topics_to_subscriber_name[t] = name + else: + self.topics_to_subscriber_name[topic] = name + return subscriber def remove_subscriber(self, name): diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py index 425fbed..a90a1a0 100644 --- a/pyrobolearn/simulators/mujoco.py +++ b/pyrobolearn/simulators/mujoco.py @@ -46,15 +46,15 @@ import xml.etree.ElementTree as ET # XML parser from xml.dom import minidom # to print in a pretty way the XML file # import mesh converter (from .obj to .stl) -try: - import pymesh # rapid prototyping platform focused on geometry processing - # doc: https://pymesh.readthedocs.io/en/latest/user_guide.html - - import pyassimp # library to import and export various 3d-model-formats - # doc: http://www.assimp.org/index.php - # github: https://github.com/assimp/assimp -except ImportError as e: - raise ImportError(str(e) + "\nTry to install pymesh pyassimp: `pip install pymesh pyassimp`") +# try: +# import pymesh # rapid prototyping platform focused on geometry processing +# # doc: https://pymesh.readthedocs.io/en/latest/user_guide.html +# +# import pyassimp # library to import and export various 3d-model-formats +# # doc: http://www.assimp.org/index.php +# # github: https://github.com/assimp/assimp +# except ImportError as e: +# raise ImportError(str(e) + "\nTry to install pymesh pyassimp: `pip install pymesh pyassimp`") # import image converter from PIL import Image diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index bfa7342..ea6b037 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -2645,6 +2645,42 @@ class Simulator(object): 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. + """ + # if a middleware is defined + if self.middleware is not None and self._middleware_enabled: + middleware_id = self._middleware_ids[body_id] + + # get jacobian from the middleware + jacobian = self.middleware.get_jacobian(middleware_id, link_id, local_position, q) + if jacobian is not None: + return jacobian + + # get the jacobian from the simulator (if we don't have a middleware or didn't get the jacobian from it) + return self._calculate_jacobian(body_id, link_id, local_position, q, dq, des_ddq) + + 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. @@ -2674,6 +2710,39 @@ class Simulator(object): :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 + """ + # if a middleware is defined + if self.middleware is not None and self._middleware_enabled: + middleware_id = self._middleware_ids[body_id] + + # get inertia matrix from the middleware + inertia = self.middleware.get_inertia_matrix(middleware_id, q) + if inertia is not None: + return inertia + + # get the inertia matrix from the simulator (if we don't have a middleware or didn't get the inertia from it) + return self._calculate_mass_matrix(body_id, q) + + 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 diff --git a/pyrobolearn/utils/__init__.py b/pyrobolearn/utils/__init__.py index b1392d2..992f5ca 100644 --- a/pyrobolearn/utils/__init__.py +++ b/pyrobolearn/utils/__init__.py @@ -1,7 +1,8 @@ # -*- coding: utf-8 -*- - import inspect import types +import functools +import re import numpy as np # import data structures @@ -29,6 +30,9 @@ from . import feedback # import real-time plotting from . import plotting +# import parsers +from . import parsers + # import parsers # from . import parsers @@ -40,6 +44,72 @@ def has_attribute(object, name): return hasattr(object, name) +def rsetattr(obj, attr, val): + """ + Recursively set an attribute. This is useful for nested sub-objects or chained properties. + + Examples: + class A(object): + def __init__(self, a=0): + self.a = a + + class B(object): + def __init__(self, b): + self.b = b + + obj = B(b=A()) + rsetattr(obj, 'b.a', 1) # obj.b.a is set to 1 now + + References: + - https://stackoverflow.com/questions/31174295/getattr-and-setattr-on-nested-subobjects-chained-properties + """ + pre, _, post = attr.rpartition('.') + return setattr(rgetattr(obj, pre) if pre else obj, post, val) + + +def rgetattr(obj, attr, *args): + """ + Recursively get an attribute. This is useful for nested subobjects or chained properties. + + Examples: + class A(object): + def __init__(self, a=0): + self.a = a + + class B(object): + def __init__(self, b): + self.b = b + + obj = B(b=A()) + rgetattr(obj, 'b.a') # this will access obj.b.a and thus print 0 + + References: + - https://stackoverflow.com/questions/31174295/getattr-and-setattr-on-nested-subobjects-chained-properties + """ + def _getattr(obj, attr): + search = re.search("\[.*\]", attr) # check for square brackets + if search is not None: + begin_idx, last_idx = search.span() + a = getattr(obj, attr[:begin_idx], *args) + idx = last_idx + while idx != begin_idx: + idx = attr.find(']', begin_idx) + s = attr[begin_idx+1:idx] + if re.match("[0-9]+", s): # check if numeric string + if ':' in s: # check for ':', i.e. slice + sl = slice(*[{True: lambda n: None, False: int}[x == ''](x) + for x in (s.split(':') + ['', '', ''])[:3]]) + a = a[sl] + else: # if no slice + a = a[int(s)] + else: # string for dictionary + a = a[s] + begin_idx = idx + return a + return getattr(obj, attr, *args) + return functools.reduce(_getattr, [obj] + attr.split('.')) + + def has_variable(object, name): """Check if the given object has a variable with the given name""" attribute = getattr(object, name, None)