///|
/// Typed RL task metadata: the observation/action/reward contract a policy
/// trainer needs, exported as `rl_task.json` next to the MJCF model.
///|
/// The training objective a task targets.
#derive(Debug, Eq)
pub(all) enum RlTaskKind {
Balance
WheeledLocomotion
QuadrupedWalk
JumpAndLevel
Custom(String)
}
///|
pub fn RlTaskKind::to_string(self : RlTaskKind) -> String {
match self {
Balance => "balance"
WheeledLocomotion => "wheeled_locomotion"
QuadrupedWalk => "quadruped_walk"
JumpAndLevel => "jump_and_level"
Custom(name) => name
}
}
///|
/// Domain randomization ranges and disturbances applied on reset.
#derive(Debug, Eq)
pub(all) struct DomainRandSpec {
mass_scale_range : (Double, Double)
friction_range : (Double, Double)
kp_scale_range : (Double, Double)
push_impulse_ns : Double
obs_noise_std : Double
}
///|
pub fn DomainRandSpec::new(
mass_scale_range~ : (Double, Double) = (0.9, 1.1),
friction_range~ : (Double, Double) = (0.7, 1.3),
kp_scale_range~ : (Double, Double) = (0.9, 1.1),
push_impulse_ns~ : Double = 0.0,
obs_noise_std~ : Double = 0.0,
) -> DomainRandSpec {
{
mass_scale_range,
friction_range,
kp_scale_range,
push_impulse_ns,
obs_noise_std,
}
}
///|
/// Weighted reward terms. Negative weights penalize their quantity.
#derive(Debug, Eq)
pub(all) struct RewardWeights {
forward_vel : Double
target_height : Double
upright : Double
energy : Double
smoothness : Double
alive : Double
}
///|
pub fn RewardWeights::new(
forward_vel~ : Double = 1.0,
target_height~ : Double = 1.0,
upright~ : Double = 1.0,
energy~ : Double = -0.01,
smoothness~ : Double = -0.01,
alive~ : Double = 0.5,
) -> RewardWeights {
{ forward_vel, target_height, upright, energy, smoothness, alive, }
}
///|
/// One robot's RL contract.
#derive(Debug, Eq)
pub(all) struct RlTaskSpec {
kind : RlTaskKind
control_dt_s : Double
episode_len_s : Double
target_base_height_m : Double
target_vel_x_mps : Double
termination_tilt_deg : Double
termination_min_z_m : Double
imu_link : String
wheel_links : Array[String]
hip_joints : Array[String]
wheel_joints : Array[String]
action_scales : Map[String, Double]
rewards : RewardWeights
domain_rand : DomainRandSpec
}
///|
pub fn RlTaskSpec::new(
kind~ : RlTaskKind = WheeledLocomotion,
control_dt_s~ : Double = 0.01,
episode_len_s~ : Double = 10.0,
target_base_height_m~ : Double = 0.30,
target_vel_x_mps~ : Double = 0.5,
termination_tilt_deg~ : Double = 55.0,
termination_min_z_m~ : Double = 0.15,
imu_link~ : String = "",
wheel_links~ : Array[String] = [],
hip_joints~ : Array[String] = [],
wheel_joints~ : Array[String] = [],
action_scales~ : Map[String, Double] = Map([]),
rewards~ : RewardWeights = RewardWeights::new(),
domain_rand~ : DomainRandSpec = DomainRandSpec::new(),
) -> RlTaskSpec {
{
kind,
control_dt_s,
episode_len_s,
target_base_height_m,
target_vel_x_mps,
termination_tilt_deg,
termination_min_z_m,
imu_link,
wheel_links,
hip_joints,
wheel_joints,
action_scales,
rewards,
domain_rand,
}
}
///|
pub fn Component::with_rl_task(
self : Component,
spec : RlTaskSpec,
) -> Component {
{ ..self, rl_task: Some(spec), }
}
///|
fn rl_reward_json(weights : RewardWeights) -> @json.Json {
inspect_object([
("forward_vel", @json.Json::number(weights.forward_vel)),
("target_height", @json.Json::number(weights.target_height)),
("upright", @json.Json::number(weights.upright)),
("energy", @json.Json::number(weights.energy)),
("smoothness", @json.Json::number(weights.smoothness)),
("alive", @json.Json::number(weights.alive)),
])
}
///|
fn rl_domain_rand_json(spec : DomainRandSpec) -> @json.Json {
inspect_object([
(
"mass_scale_range",
@json.Json::array([
@json.Json::number(spec.mass_scale_range.0),
@json.Json::number(spec.mass_scale_range.1),
]),
),
(
"friction_range",
@json.Json::array([
@json.Json::number(spec.friction_range.0),
@json.Json::number(spec.friction_range.1),
]),
),
(
"kp_scale_range",
@json.Json::array([
@json.Json::number(spec.kp_scale_range.0),
@json.Json::number(spec.kp_scale_range.1),
]),
),
("push_impulse_ns", @json.Json::number(spec.push_impulse_ns)),
("obs_noise_std", @json.Json::number(spec.obs_noise_std)),
])
}
///|
/// One actuator record: the joint, the MJCF actuator kind, gains, and the
/// control range. Mirrors the options written into `bundle/mjcf/*.xml`.
fn rl_actuator_json(
robot : @urdf.Robot,
binding : ControlBinding,
spec : RlTaskSpec,
) -> @json.Json? {
match robot.get_joint(binding.joint) {
None => None
Some(joint) => {
let (effort, velocity, lower, upper) = match joint.limit {
Some(limit) => (limit.effort, limit.velocity, limit.lower, limit.upper)
None => (0.0, 0.0, None, None)
}
let (kind, kp, kv, ctrl_min, ctrl_max) = match joint.joint_type {
Continuous => {
let vmax = if velocity > 0.0 { velocity } else { 10.0 }
("velocity", 0.0, 2.0, -vmax, vmax)
}
Revolute | Prismatic => {
let lo = match lower {
Some(value) => value
None => -3.141592653589793
}
let hi = match upper {
Some(value) => value
None => 3.141592653589793
}
("position", 70.0, 3.0, lo, hi)
}
_ => ("motor", 0.0, 0.0, -effort, effort)
}
let entries : Array[(String, @json.Json)] = [
("joint", @json.Json::string(joint.name)),
(
"name",
@json.Json::string(
if binding.actuator == "" {
joint.name + "_" + kind
} else {
binding.actuator
},
),
),
("kind", @json.Json::string(kind)),
("ctrl_min", @json.Json::number(ctrl_min)),
("ctrl_max", @json.Json::number(ctrl_max)),
(
"scale",
@json.Json::number(spec.action_scales.get(joint.name).unwrap_or(1.0)),
),
]
if kp > 0.0 {
entries.push(("kp", @json.Json::number(kp)))
}
if kv > 0.0 {
entries.push(("kv", @json.Json::number(kv)))
}
if effort > 0.0 {
entries.push(("forcerange", @json.Json::number(effort)))
}
Some(inspect_object(entries))
}
}
}
///|
fn rl_range_valid(range : (Double, Double)) -> Bool {
range.0 >= 0.0 && range.0 <= range.1
}
///|
/// Validation for an attached RL spec. Empty means consistent.
pub fn Component::rl_task_checks(self : Component) -> Array[String] {
let warnings : Array[String] = []
let spec = match self.rl_task {
Some(spec) => spec
None => return warnings
}
let robot = self.to_robot()
let bindings = self.control_bindings()
let bound : Array[String] = bindings.map(fn(binding) { binding.joint })
if spec.control_dt_s <= 0.0 {
warnings.push(self.name + ": rl task control_dt_s must be > 0")
}
if spec.episode_len_s <= 0.0 {
warnings.push(self.name + ": rl task episode_len_s must be > 0")
}
if spec.termination_tilt_deg <= 0.0 {
warnings.push(self.name + ": rl task termination tilt must be > 0")
}
if spec.termination_min_z_m < 0.0 {
warnings.push(self.name + ": rl task termination min z must be >= 0")
}
if bindings.length() == 0 {
warnings.push(
self.name +
": rl task has no commandable actuators (add control bindings)",
)
}
for name in spec.hip_joints {
if !bound.contains(name) {
warnings.push(
self.name +
": rl task hip joint '" +
name +
"' is not a commandable joint",
)
}
}
for name in spec.wheel_joints {
if !bound.contains(name) {
warnings.push(
self.name +
": rl task wheel joint '" +
name +
"' is not a commandable joint",
)
}
}
for name, _scale in spec.action_scales {
if !bound.contains(name) {
warnings.push(
self.name +
": rl task action scale refers to unknown joint '" +
name +
"'",
)
}
}
if spec.imu_link != "" && robot.get_link(spec.imu_link) is None {
warnings.push(
self.name +
": rl task imu link '" +
spec.imu_link +
"' is not a robot link",
)
}
if !rl_range_valid(spec.domain_rand.mass_scale_range) ||
spec.domain_rand.mass_scale_range.0 <= 0.0 {
warnings.push(self.name + ": rl task mass scale range is invalid")
}
if !rl_range_valid(spec.domain_rand.friction_range) {
warnings.push(self.name + ": rl task friction range is invalid")
}
if !rl_range_valid(spec.domain_rand.kp_scale_range) ||
spec.domain_rand.kp_scale_range.0 <= 0.0 {
warnings.push(self.name + ": rl task kp scale range is invalid")
}
if spec.domain_rand.push_impulse_ns < 0.0 {
warnings.push(self.name + ": rl task push impulse must be >= 0")
}
if spec.domain_rand.obs_noise_std < 0.0 {
warnings.push(self.name + ": rl task observation noise must be >= 0")
}
warnings
}
///|
/// The RL task contract as a JSON document. `"null"` when no task is attached.
pub fn Component::rl_task_json(
self : Component,
joint_positions~ : Map[String, Double] = Map([]),
) -> String {
let spec = match self.rl_task {
Some(spec) => spec
None => return "null"
}
let robot = self.to_robot()
let bindings = self.control_bindings()
let actuators : Array[@json.Json] = []
let action_joints : Array[@json.Json] = []
for binding in bindings {
match rl_actuator_json(robot, binding, spec) {
Some(entry) => {
actuators.push(entry)
action_joints.push(entry)
}
None => ()
}
}
let count = actuators.length()
let obs_dim = 10 + 3 * count
let home : Map[String, @json.Json] = Map([])
for name, value in joint_positions {
home.set(name, @json.Json::number(value))
}
inspect_object([
("name", @json.Json::string(self.display_name())),
("kind", @json.Json::string(spec.kind.to_string())),
(
"mjcf_path",
@json.Json::string("bundle/mjcf/" + self.display_name() + ".xml"),
),
("control_dt_s", @json.Json::number(spec.control_dt_s)),
("episode_len_s", @json.Json::number(spec.episode_len_s)),
("target_base_height_m", @json.Json::number(spec.target_base_height_m)),
("target_vel_x_mps", @json.Json::number(spec.target_vel_x_mps)),
(
"termination",
inspect_object([
("tilt_deg", @json.Json::number(spec.termination_tilt_deg)),
("min_z_m", @json.Json::number(spec.termination_min_z_m)),
]),
),
("imu_link", @json.Json::string(spec.imu_link)),
(
"wheel_links",
@json.Json::array(
spec.wheel_links.map(fn(name) { @json.Json::string(name) }),
),
),
(
"hip_joints",
@json.Json::array(
spec.hip_joints.map(fn(name) { @json.Json::string(name) }),
),
),
(
"wheel_joints",
@json.Json::array(
spec.wheel_joints.map(fn(name) { @json.Json::string(name) }),
),
),
("home_qpos", @json.Json::object(home)),
("actuators", @json.Json::array(actuators)),
(
"obs_spec",
inspect_object([
("base_height", @json.Json::number(1.0)),
("projected_gravity", @json.Json::number(3.0)),
("base_ang_vel", @json.Json::number(3.0)),
("base_lin_vel", @json.Json::number(3.0)),
("joint_pos", @json.Json::number(count.to_double())),
("joint_vel", @json.Json::number(count.to_double())),
("prev_action", @json.Json::number(count.to_double())),
("dim", @json.Json::number(obs_dim.to_double())),
]),
),
(
"action_spec",
inspect_object([
("dim", @json.Json::number(count.to_double())),
("joints", @json.Json::array(action_joints)),
]),
),
("rewards", rl_reward_json(spec.rewards)),
("domain_rand", rl_domain_rand_json(spec.domain_rand)),
]).stringify()
}