Environment Modeling

note

Since Lively is still in beta, the design is subject to change and should not be considered final!

We have also created examples in Javascript, Python, and Rust for environmental modeling example. You can find the file by clicking the links in the table down below.

Language	Path	Command to run the example
Rust	link	cargo run --package lively --example environmental_modeling_example
Python	link	run in the Jupyter Notebook
Javascript	link	yarn build, yarn dev

The environmental shapes introduced at the beginning of the solver can be considered as static and cannot be modified. The Cylinder in this example is an example of static shape. For more information please see shape_update

Live

Loading Example...

Javascript

import { panda, ur3e } from './urdfs.js';
import { Solver } from '@people_and_robots/lively';

const initialRootBounds = [
  // An exmaple of root bounds
  { value: 0.0, delta: 0.0 },
  { value: 0.25, delta: 0.0 },
  { value: 0.5, delta: 0.0 },
  { value: 0.0, delta: 0.0 },
  { value: 0.0, delta: 0.0 },
  { value: 0.0, delta: 0.0 },
];

const shapeUpdates = [
  {
    Add: {
      id: 'env-box', // must be an unique id
      shape: {
        type: 'Box', //can be 'Cylinder', 'Capsule', or 'Sphere'
        name: 'box', // name can be arbitrary
        frame: 'world', // or 'world'
        physical: true, // physical collision
        x: 0.25,
        y: 0.25,
        z: 0.25, // dimension of the box
        localTransform: {
          translation: [0.6, 0.05, 0.15],
          rotation: [0.0, 0.0, 0.0, 1.0],
        },
      },
    },
  },
  {
    Move: {
      id: 'env-box',
      transform: {
        translation: [0.6, 0.05, 0.15],
        rotation: [0.0, 0.0, 0.0, 1.0],
      },
    },
  },
  { Delete: 'env-box' },
];

const initialObjectives = {
  // some objective examples. Notice for JavaScript, you do not need to import anything for objective. Simply construct an object
  smoothness: {
    name: 'MySmoothnessObjective',
    type: 'SmoothnessMacro',
    weight: 15,
    joints: true,
    origin: false,
    links: true,
  },
  collision: {
    // The main objective that allows the robot to avoid collision within the links, as well as with the environmental objects.
    name: 'MyCollisionDetection',
    type: 'CollisionAvoidance',
    weight: 3,
  },
  jointLimit: {
    name: 'MyJointLimit',
    type: 'JointLimits',
    weight: 5,
  },
  position: {
    // The main objective that allows the pand hand to follow the position defined by the sphere transform control visual in the scene.
    name: 'MyPositionMatchObjective',
    type: 'PositionMatch',
    link: 'tool0',
    weight: 15,
  },
  orientation: {
    // The main objective that allows the pand hand to follow the orientation defined by the arrow transform control visual in the scene.
    name: 'MyOrientationMatchObjective',
    type: 'OrientationMatch',
    link: 'tool0',
    weight: 10,
  },
};

const initialEnvShapes = [
  {
    type: 'Cylinder', // The Cylinder here is an example of static environmental shape. This shape will be not able to be moved or deleted.
    name: 'pill',
    frame: 'world',
    physical: true,
    length: 0.3,
    radius: 0.2,
    localTransform: {
      translation: [-0.8, 0.0, 0.1],
      rotation: [1.0, 0.0, 0.0, 0.0],
    }, // [x, y, z, w] ordering for quaternion
  },
];

const collision_settings = {
  // This is an example of customized collision_settings
  dMax: 0.1,
  r: 0.0,
  aMax: 2.0,
  timeBudget: 100,
  timed: false,
};

const newSolver = new Solver(
  ur3e,
  initialObjectives,
  initialRootBounds,
  initialEnvShapes,
  null,
  null,
  null,
  collision_settings
);

newSolver.computeAverageDistanceTable();
const d = new Date();
let time = d.getTime(); // Get the time used in Math.sin
let goal = {
  // An goal example with defined Scalar and Size values for the lively objectives
  position: {
    Translation: [0.6, 0, 0.6],
  },
  finger_joint_control: {
    Scalar: 0.02,
  },
  positionLively: {
    Size: [0.07, 0.05, 0.08],
  },
  orientationLively: {
    Size: [0.07, 0.05, 0.08],
  },
  jointLimit: {
    Scalar: 0.02,
  },
};
const newState = newSolver.solve(goal, {}, time / 1000, shapeUpdates);

document.querySelector('#app').innerHTML = `
  <div>
   ${JSON.stringify(newState)}
  </div>`;

console.log(newState);

Python

from lively import Solver, SmoothnessMacroObjective

# Instantiate a new solver
solver = Solver(
  urdf='<?xml version="1.0" ?><robot name="panda">...</robot>', # Full urdf as a string
  objectives={
      # An example objective (smoothness macro)
      "smoothness":SmoothnessMacroObjective(name="MySmoothnessObjective",weight=5)
  },
  root_bounds=[
      ScalarRange(value=0.0,delta=0.0),ScalarRange(value=0.0,delta=0.0),ScalarRange(value=0.0,delta=0.0), # Translational, (x, y, z)
      ScalarRange(value=0.0,delta=0.0),ScalarRange(value=0.0,delta=0.0),ScalarRange(value=0.0,delta=0.0)  # Rotational, (r, p, y)
  ],
  shapes=[
      BoxShape(name="Table",frame="world",physical=True,x=2,y=1,z=1.2,local_transform=Transform.isometry())
  ],
)
add_box = BoxShape(name="box_1",frame="world",physical=True,x=1,y=1,z=1.2,local_transform=Transform.isometry())
# This is an Add ShapeUpdate functionality or type. This introduce a new Cube with "box_1" as the id to the environment.
shape_update = [pyShapeUpdate.Add(id = "box_1", shape = add_box)]
# Run solve to get a solved state
state = solver.solve(goals = {}, weights = {}, time = 0.0, shape_update = shape_update);
# Log the initial state
print(state)

Rust

use lively::lively::Solver;
use lively::objectives::core::base::CollisionAvoidanceObjective;
use lively::objectives::core::base::SmoothnessMacroObjective;
use lively::objectives::objective::Objective;
use lively::utils::info::AddShape;
use lively::utils::info::ShapeUpdate;
use lively::utils::shapes::*;
use nalgebra::geometry::Translation3;
use nalgebra::Isometry3;
use nalgebra::Quaternion;
use nalgebra::UnitQuaternion;
use std::collections::HashMap;
use std::fs;

fn main() {
    let mut objectives: HashMap<String, Objective> = HashMap::new();
    // Add a Smoothness Macro Objective
    objectives.insert(
        "smoothness".into(),
        // An example objective (smoothness macro)
        Objective::SmoothnessMacro(SmoothnessMacroObjective::new(
            "MySmoothnessObjective".to_string(),
            5.0,
            true,
            false,
            false,
        )),
    );
    // Add Collision Avoidance Objective
    objectives.insert(
        "collision_avoidance".into(),
        Objective::CollisionAvoidance(CollisionAvoidanceObjective::new(
            "MyCollisionAvoidance".to_string(),
            10.0,
        )),
    );

    let iso_1 = Isometry3::from_parts(
        // defining transform from translation and rotation
        Translation3::new(
            1.7497281999999998,
            -0.24972819999999987,
            0.050000000000000044,
        ),
        UnitQuaternion::from_quaternion(Quaternion::new(
            0.0,
            0.0,
            -0.7069999677447771,
            0.7072135784958345,
        )),
    );
    //box_1 here is a static environmental shape. This means that box_1 can not be moved or deleted.
    let box_1 = Shape::Box(BoxShape::new(
        //can be 'CylinderShape', 'CapsuleShape', or 'SphereShape'
        "conveyorCollisionShapeBase".to_string(), // name can be arbitrary
        "world".to_string(),                      // frame name
        true,                                     // physical collision
        1.0,                                      // dimension of the box
        1.1,
        1.7,
        iso_1, // local_transform of the box
    ));
    let mut box_shapes_vec: Vec<Shape> = vec![box_1];
    let data =
        fs::read_to_string("./tests/basic.xml").expect("Something went wrong reading the file");
    let mut solver = Solver::new(
        data.clone(),         // Full urdf as a string
        objectives,           // objectives
        None,                 //root_bounds
        Some(box_shapes_vec), //static environmental shapes
        None,                 // inital_state
        None,                 //max_retries
        None,                 //max_iterations
        None,
    ); //collision_settings

    let iso_2 = Isometry3::from_parts(
        Translation3::new(-1.0, 0.5, 2.1),
        UnitQuaternion::from_quaternion(Quaternion::new(
            0.0,
            0.0,
            -0.7069999677447771,
            0.7072135784958345,
        )),
    );
    //box_2 here is not a static environmental shape because it will be added to the environment by shape_update
    let box_2 = Shape::Box(BoxShape::new(
        "panda_shapebox".to_string(),
        "world".to_string(),
        true,
        1.5,
        1.7,
        1.0,
        iso_2,
    ));

    // add box_2 to the environment.
    let add_shape_update = AddShape {
        id: 1.to_string(), //The id must be unique
        shape: box_2.clone(),
    };
    let shape_update: Vec<ShapeUpdate> = vec![ShapeUpdate::Add(add_shape_update)]; // shape_update

    // Run solve to get a solved state
    let state = solver.solve(HashMap::new(), HashMap::new(), 0.0, Some(shape_update));
    // Log the initial state
    println!("{:?}", state);
}