import gym
import gym_ignition_models
import numpy as np
import pytest
from gazebo_scenario_plugins import plugins
from gym_ignition import utils
from gym_ignition.utils.scenario import init_gazebo_sim
from scenario import core as scenario_core
# Set the verbosity
utils.logger.set_level(gym.logger.DEBUG)
# Panda PID gains
# https://github.com/mkrizmancic/franka_gazebo/blob/master/config/default.yaml
panda_pid_gains_1000Hz = {
"panda_joint1": scenario_core.PID(50, 0, 20),
"panda_joint2": scenario_core.PID(10000, 0, 500),
"panda_joint3": scenario_core.PID(100, 0, 10),
"panda_joint4": scenario_core.PID(1000, 0, 50),
"panda_joint5": scenario_core.PID(100, 0, 10),
"panda_joint6": scenario_core.PID(100, 0, 10),
"panda_joint7": scenario_core.PID(10, 0.5, 0.1),
"panda_finger_joint1": scenario_core.PID(100, 0, 50),
"panda_finger_joint2": scenario_core.PID(100, 0, 50),
}
def test_actuation_delay():
# Get the simulator and the world
gazebo, world = init_gazebo_sim(step_size=0.001,
def __init__(self,
world: scenario.World,
position: List[float] = (0.0, 0.0, 0.0),
orientation: List[float] = (1.0, 0, 0, 0),
model_file: str = None):
# Get a unique model name
model_name = get_unique_model_name(world, "panda")
# Initial pose
initial_pose = scenario.Pose(position, orientation)
# Get the default model description (URDF or SDF) allowing to pass a custom model
if model_file is None:
model_file = Panda.get_model_file()
# Insert the model
ok_model = world.to_gazebo().insert_model(model_file,
initial_pose,
model_name)
if not ok_model:
raise RuntimeError("Failed to insert model")
# Get the model
model = world.get_model(model_name)
# Initial joint configuration
model.to_gazebo().reset_joint_positions(
[0, -0.785,0, -2.356, 0, 1.571, 0.785],
[name for name in model.joint_names() if "panda_joint" in name])
# From:
# https://github.com/mkrizmancic/franka_gazebo/blob/master/config/default.yaml
pid_gains_1000hz = {
'panda_joint1': scenario.PID(50, 0, 20),
'panda_joint2': scenario.PID(10000, 0, 500),
'panda_joint3': scenario.PID(100, 0, 10),
'panda_joint4': scenario.PID(1000, 0, 50),
'panda_joint5': scenario.PID(100, 0, 10),
'panda_joint6': scenario.PID(100, 0, 10),
'panda_joint7': scenario.PID(10, 0.5, 0.1),
'panda_finger_joint1': scenario.PID(100, 0, 50),
'panda_finger_joint2': scenario.PID(100, 0, 50),
}
# Check that all joints have gains
if not set(model.joint_names()) == set(pid_gains_1000hz.keys()):
raise ValueError("The number of PIDs does not match the number of joints")
# Set the PID gains
for joint_name, pid in pid_gains_1000hz.items():
if not model.get_joint(joint_name).set_pid(pid=pid):
raise RuntimeError(f"Failed to set the PID of joint '{joint_name}'")
# Set the default PID update period
assert model.set_controller_period(1000.0)
# Initialize base class
super().__init__(model=model)
pytestmark = pytest.mark.scenario
import numpy as np
from typing import Tuple
import gym_ignition_models
from scenario import core
from scenario import gazebo as scenario
from ..common.utils import default_world_fixture as default_world
# Set the verbosity
scenario.set_verbosity(scenario.Verbosity_debug)
# Panda PID gains
# https://github.com/mkrizmancic/franka_gazebo/blob/master/config/default.yaml
panda_pid_gains_1000Hz = {
'panda_joint1': core.PID(50, 0, 20),
'panda_joint2': core.PID(10000, 0, 500),
'panda_joint3': core.PID(100, 0, 10),
'panda_joint4': core.PID(1000, 0, 50),
'panda_joint5': core.PID(100, 0, 10),
'panda_joint6': core.PID(100, 0, 10),
'panda_joint7': core.PID(10, 0.5, 0.1),
'panda_finger_joint1': core.PID(100, 0, 50),
'panda_finger_joint2': core.PID(100, 0, 50),
}
@pytest.mark.parametrize("default_world", [(1.0 / 1_000, 1.0, 1)], indirect=True)
def test_position_pid(default_world: Tuple[scenario.GazeboSimulator, scenario.World]):
# Get the simulator and the world