Is it possible to warp models (robots) into a certain pose? I.e., is it possible to directly set link positions?

[FirefoxMetzger]

I searched the documentation and I found methods for reading the current pose/velocity/acceleration as well as setting targets for each. However, I didn't find a way to directly set the position of a joint that would effectively teleport the model into the desired pose.

My use-case is to programmatically set a robot's pose (panda in my case) into a specific joint configuration before the simulation starts.

[AndrejOrsula]

Hello, I believe you are looking for Model::resetJointPositions(). Alternatively, there is Joint::resetPosition() if you need to access a single joint (which you can also do with Model::resetJointPositions() by passing jointNames argument).

When resetting positions, I also recommend setting velocities to 0.0 for the affected joints (Model::resetJointVelocities()), especially if the robot was in motion. Without this, I have experienced occasional/unpredictable segfault during subsequent Gazebo PostUpdate call - even if the robot was seemingly static (not sure if it still occurs).

Remember to run a paused Gazebo step for this to take effect. In case your model uses plugins that update only when the simulation is running, e.g. ignition::gazebo::systems::JointStatePublisher, you need to run an unpaused Gazebo step for such plugins to update.

If you are using Python bindings, it might look something like this:

robot_name      = 'panda'
joint_positions = (0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.79, 0.04, 0.04)

robot = world.get_model(robot_name).to_gazebo()
if not robot.reset_joint_positions(joint_positions):
    raise RuntimeError("Failed to reset robot joint positions")
if not robot.reset_joint_velocities([0.0] * len(joint_positions)):
    raise RuntimeError("Failed to reset robot joint velocities")

if not gazebo.run(paused=True):
    raise RuntimeError("Failed to execute a paused Gazebo run")

Edit: Updated Python snippet to include .to_gazebo() dynamic cast as explained by the original author in a reply below.

[FirefoxMetzger]

This is indeed what I've been looking for. Thanks!

I think the name reset_joint_positions is a little confusing here because it implies the existence of a default position the robot is being reset to (one would also expect no arguments). Instead, the robot is actually being set into a position given by a mandatory input argument.

@traversaro Is there a lot of code depending on reset_joint_X? A change to streamline the API could be to rename it to set_joint_X (no longer implying a default) and potentially introduce a reset_joint_X with no arguments if there is a default (home ?) position. For backward compatibility, this could be set_joint_position(position, *, jointNames=None) (new) and reset_joint_positions(*, position=None, jointNames=None) to allow legacy code to keep calling reset_joint_positions(position, ...), and then forward to set_joint_positions if position is not None together with a depreciation warning.

[traversaro]

I guess @diegoferigo is the one that can provide the more informed reply.

[diegoferigo]

This is indeed what I've been looking for. Thanks!

I think the name reset_joint_positions is a little confusing here because it implies the existence of a default position the robot is being reset to (one would also expect no arguments). Instead, the robot is actually being set into a position given by a mandatory input argument.

The default position exists and it's what specified in the urdf / sdf model, but it's not used explicitly in this method (i.e. it is not its default argument when missing). This could be however a useful addition that does not break the APIs.

[diegoferigo]

Also, in the snippets above, make sure that the Model or Joint objects you have are from scenario.bindings.gazebo.* and not scenario.bindings.core.*. In fact, the reset* methods are implementation details of the Gazebo backend, and they are not part of the ScenarIO APIs.

In python, we offer helpers to perform the dynamic cast:

# Note: type hinting not required, it's just to clarify what's happening
model: scenario_core.Model = world.get_model("my_model")
model_gazebo: scenario_gazebo.Model = model.to_gazebo()
model_gazebo.reset_joint_positions([1,2,3])