wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-12 12:41:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

update ROS robot middlewares

Browse Source
This commit is contained in:
Brian Delhaisse
2019-10-22 08:11:23 +02:00
parent e660e9d8c4
commit 605b2c8194
12 changed files with 1056 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/middlewares/README.md
+1 -1
View File
@@ -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.
examples/middlewares/bullet_ros_control_gazebo.py
+4 -4
View File
@@ -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/<robot_name>/` folder using the `roslaunch <robot>_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():
pyrobolearn/simulators/middlewares/robot_middleware.py
+40 -5
View File
@@ -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
pyrobolearn/simulators/middlewares/robots/franka.py
+113 -11
View File
@@ -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
pyrobolearn/simulators/middlewares/robots/iiwa14.py
+283
View File
@@ -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
pyrobolearn/simulators/middlewares/robots/rrbot.py
+255
View File
@@ -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
pyrobolearn/simulators/middlewares/ros.py
+143 -89
View File
@@ -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 '/<robot_name>/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>/robot.urdf
dirname = str(os.path.dirname(path)) # /path/to/pyrobolearn/robots/urdfs/<robot>/
# dirname = str(os.path.dirname(path)) # /path/to/pyrobolearn/robots/urdfs/<robot>/
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>/
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):
"""
pyrobolearn/simulators/middlewares/ros_publisher.py
+34 -22
View File
@@ -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
pyrobolearn/simulators/middlewares/ros_subscriber.py
+34 -23
View File
@@ -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):
pyrobolearn/simulators/mujoco.py
+9 -9
View File
@@ -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
pyrobolearn/simulators/simulator.py
+69
View File
@@ -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
pyrobolearn/utils/__init__.py
+71 -1
View File
@@ -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)