(Pythonic) Quality of Life additions to Panda
FirefoxMetzger opened this issue · 3 comments
When loading panda from gym_ignition_environments.models.panda.Panda I've noticed that the resulting object could be a little more pythonic. What I ended up doing locally is to write a sub-class with getters and setters for common properties such as joint_position_targets, etc. that take care of conversion to/from numpy and enforces constraints. I've also added the constraints as parameters of the object (for easy reference).
In a nutshell, this allows you to do things like
import numpy as np
# ...
gazebo = scenario.GazeboSimulator(...)
panda = Panda(...) # same arguments as parent
# set a random joint pose
panda.position = np.random.rand(9) * (panda.max_position - panda.min_position) + panda.min_position
gazebo.run(paused=True)
print(f"Pandas position is {panda.position}")
#the same applies to velocity and targets of each
panda.velocity = np.zeros(panda.dofs)
panda.target_position = panda.home_position
# ...
# or shorter
panda.reset() # resets the position, velocity, and target pos/vel/acc
panda.tool # shorthand for the end-effector's link
curent_pos = panda.tool.position()My question is if upstream (here) is interested in these additions. A reason to not include it could be that it somewhat breaks with the style of the purely swigg wrapped objects, i.e., it might be counterintuitive why these nice things work on panda, but none of the other models in the simulation. If there is interest, I can prepare a PR that enhances Panda.py, though I won't have time to write tests until late this month.
Here is the class for reference:
Panda
class Panda(gym_ignition_environments.models.panda.Panda):
def __init__(self, **kwargs):
super().__init__(**kwargs)
self.home_position = (0, -0.785,0, -2.356, 0, 1.571, 0.785, 0.2, 0.2)
# joint constraints (joint-space, units in rad, e.g. rad/s for velocity)
# TODO: check the values of the fingers, these are all guesses
self.max_position = np.array((2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973, 0.5, 0.5))
self.min_position = np.array((-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973, 0, 0))
self.max_velocity = np.array((2.1750, 2.1750, 2.1750, 2.1750, 2.6100, 2.6100, 2.6100, 2, 2))
self.min_velocity = - self.max_velocity
self.max_acceleration = np.array((15, 7.5, 10, 12.5, 15, 20, 20, 10, 10), dtype=np.float_)
self.min_acceleration = - self.max_acceleration
self.max_jerk = np.array((7500, 3750, 5000, 6250, 7500, 10000, 10000, 10000, 10000), dtype=np.float_)
self.min_jerk = - self.max_jerk
self.max_torque = np.array((87, 87, 87, 87, 12, 12, 12, 12, 12), dtype=np.float_)
self.min_torque = - self.max_torque
self.max_rotatum = np.array([1000] * 9)
self.min_rotatum = - self.max_rotatum
# tool constraints (cartesian)
self.max_tool_velocity = 1.7 # m/s
self.max_tool_acceleration = 13 # m/s
self.max_tool_jerk = 6500 # m/s
self.max_tool_angular_velocity = 2.5 # rad/s
self.max_tool_angular_acceleration = 25 # rad/s
self.max_tool_angular_jerk = 12500 # rad/s
# ellbow constraints (cartesian, in rad)
# This is in the docs, but I'm not sure how to interpret it. Perhaps it
# refers to null-space motion?
# https://frankaemika.github.io/docs/control_parameters.html
self.max_ellbow_velocity = 2.175
self.max_ellbow_acceleration = 10
self.max_ellbow_jerk = 5000
self.ik_joints = [
j.name()
for j in self.model.joints()
if j.type is not scenario_core.JointType_fixed
]
# potentially initialize a closed-loop controller here
def reset(self):
self.position = self.home_position
self.velocity = [0] * 9
self.target_position = self.home_position
self.target_velocity = [0] * 9
self.target_acceleration = [0] * 9
@property
def dofs(self):
return self.model.dofs()
@property
def tool(self):
return self.model.get_link("end_effector_frame")
@property
def position(self):
return np.array(self.model.joint_positions())
@property
def velocity(self):
return np.array(self.model.joint_velocities())
@property
def acceleration(self):
return np.array(self.model.joint_accelerations())
@position.setter
def position(self, position: npt.ArrayLike):
position = np.asarray(position).tolist()
if np.any((position < self.min_position) | (self.max_position < position)):
raise ValueError("The position exceeds the robot's limits.")
assert self.model.to_gazebo().reset_joint_positions(position.tolist())
@velocity.setter
def velocity(self, velocity: npt.ArrayLike):
velocity = np.asarray(velocity)
if np.any((velocity < self.min_velocity) | (self.max_velocity < velocity)):
raise ValueError("The velocity exceeds the robot's limits.")
assert self.model.to_gazebo().reset_joint_velocities(velocity.tolist())
@property
def target_position(self):
return np.array(self.model.joint_position_targets())
@property
def target_velocity(self):
return np.array(self.model.joint_velocity_targets())
@property
def target_acceleration(self):
return np.array(self.model.joint_acceleration_targets())
@target_position.setter
def target_position(self, position: npt.ArrayLike):
position = np.asarray(position)
if np.any((position < self.min_position) | (self.max_position < position)):
raise ValueError("The target position exceeds the robot's limits.")
assert self.model.set_joint_position_targets(position.tolist())
@target_velocity.setter
def target_velocity(self, velocity: npt.ArrayLike):
velocity = np.asarray(velocity)
if np.any((velocity < self.min_velocity) | (self.max_velocity < velocity)):
raise ValueError("The target velocity exceeds the robot's limits.")
assert self.model.set_joint_velocity_targets(velocity.tolist())
@target_acceleration.setter
def target_acceleration(self, acceleration: npt.ArrayLike):
acceleration = np.asarray(acceleration)
if np.any((acceleration < self.min_acceleration) | (self.max_acceleration < acceleration)):
raise ValueError("The target acceleration exceeds the robot's limits.")
assert self.model.set_joint_acceleration_targets(acceleration.tolist())@FirefoxMetzger Thanks for this contribution. While I think that moving towards a more idiomatic usage is a good thing, I'm not sure if it either fits or overfits the purpose of gym_ignition_environments. In fact, in the README we state:
gym_ignition_environments: Demo environments created withgym_ignitionandgym-ignition-models
that show the recommended structure.
I would not recommend downstream environments to use resources from gym_ignition_environments. They can be updated in future versions, and they can break compatibility with downstream. Also on my personal environments, I always copy and adapt the models classes, in most cases they are application-specific.
For what concerns your class, what I'd like to avoid are all those hardcoded values that are already stored in the model's SDF. Maintaining them also in the Python code is impractical. Furthermore, your class would only target Model, keeping Joint and Link as they are (returning Tuple[float]). I'd suggest to store that class in your downstream project. It should also simplify its maintenance since you don't have to interact with gym-ignition, that could slow down your activities depending on how fast/slow we are processing new PRs.
Instead, about numpy conversion, I sometimes think that the next major version of gym-ignition (v2.*) could return directly numpy types. However, doing so with SWIG is not straightforward. I experimented in the past with pybind11 that is much more powerful in this sense. It also allows minimizing the copies between Python and C++. However, this is a long-term plan, and there are also other features to consider before moving to pybind. For instance, I didn't find any simple way to copy the Doxygen documentation from C++ to Python, and this is a big stopper at the moment.
I'd suggest to store that class in your downstream project.
Perfectly fine with me :) I figured it might be too specific to fit, but - since it lives between my "end-user code" and gym-ignition I thought I bring it here and see if there is interest.
For what concerns your class, what I'd like to avoid are all those hardcoded values that are already stored in the model's SDF.
Yes. It would be much better to read them directly from the SDF/URDF and not have magic numbers. As far as I am aware, this is not so easy from python at this point, so I hardcoded them until I find a better solution. If I don't come across a better solution before I'm done with my current simulation I was thinking to simply create a parser myself. This would also help me, for example, with getting camera parameters/sensor publishing rates auto-magically. (Currently, I hardcode them in lack of a better alternative.)
the next major version of gym-ignition (v2.*) could return directly NumPy types
I think this could be a really useful feature indeed. Exposing the underlying data in a zero-copy fashion should nicely speed up many tasks.
Closing this, since it doesn't fit with gym-ignition.
For what concerns your class, what I'd like to avoid are all those hardcoded values that are already stored in the model's SDF.
Yes. It would be much better to read them directly from the SDF/URDF and not have magic numbers. As far as I am aware, this is not so easy from python at this point, so I hardcoded them until I find a better solution. If I don't come across a better solution before I'm done with my current simulation I was thinking to simply create a parser myself. This would also help me, for example, with getting camera parameters/sensor publishing rates auto-magically. (Currently, I hardcode them in lack of a better alternative.)
You can get some ideas from the SDFRandomizer class (also its test). It's quite easy to extract data and manipulate the SDF using XPath patterns, after all SDF is XML.
the next major version of gym-ignition (v2.*) could return directly NumPy types
I think this could be a really useful feature indeed. Exposing the underlying data in a zero-copy fashion should nicely speed up many tasks.
If the speed up would be noticeable, I'd probably have already implemented a variant of what discussed. Unfortunately, if you profile a complex script (e.g. the manipulation example), you'd notice that most of the time is spent in 1) stepping the physics engine and 2) computing IK. The overhead introduced by SWIG is basically 0. Perhaps, if we'd expose camera images, a zero-copy solution would provide a minimal boost. Though, it's not yet the case (and in this specific case, working with numpy would be ideal).