Dynamics Simulation and Control in Klampt Kris Hauser

Dynamics, Simulation, and Control in Klamp’t Kris Hauser ECE 383 / ME 442

Agenda • Simulation in Klamp’t • Key concepts • • Robot model’s dynamics Physics simulations Actuator / sensor emulation The control loop • Klamp’t Python API

Toolkit Components (today) Modeling Planning 3 D math & geometry Paths & trajectories Inverse Kinematics Motion planning Dynamics Forward Kinematics Path smoothing Trajectory optimization Contact mechanics Physics simulation Design tools Visualization System integration ROS interface Disk I/O JSON / Sockets Robot posing Motion design Physics simulation Path planning

Summary •

Concept #1: Robot model dynamic parameters • Each robot link has inertial parameters • • • Mass (scalar, > 0) COM (R 3, given in local coordinates) Inertia matrix (M 3 x 3, given in local coordinates, s. p. d. ) (note: can automatically estimate COM/inertia from geometry) (note: links can have 0 mass/inertia if rigidly fixed to other links) • Robot joints have dynamic limits • Velocity, acceleration, torque • Robot actuators have PID / joint friction parameters that are used in simulation (more later)

Dynamics •

Constrained Dynamics •

Python API (Robot. Model. Link, Mass) • Robot. Link. get. Mass(): returns the link’s Mass structure • Robot. Link. set. Mass(mass): sets the link’s Mass structure • get/set. Mass(mass): get/sets total mass to a float • get/set. Com(com): get/sets center of mass to a float 3 -tuple in link -local coordinates • get. Inertia(inertia): gets inertia as a 9 -vector, indicating rows of the inertia matrix in link-local coordinates • set. Inertia(inertia) sets inertia, either a float, float 3 -tuple, or float 9 -tuple. If float, sets inertia matrix to HL=I 3 x 3*inertia. If 3 -tuple, sets matrix to HL=diag(inertia)

Python API (Robot. Model) •

Concept #2: Simulator, controller • A Simulator generates an approximation of the physics of a controlled robot and uncontrolled rigid objects, including effects of contact • Klamp’t uses a hacked Open Dynamics Engine as underlying rigid body engine • Approximation of Coulomb friction • Custom contact handling makes trimesh-trimesh collisions more robust • Simulation time step parameter affects stability / running time • A Controller reads from simulated sensors and writes to simulated motors at a fixed time step • Sensors: time, configuration, velocity, accelerometers, force/torque sensors, etc. • Motors: motion queue control, PID control, torque control • Time step is independent of simulator timestep

Control Loop Simulator Actuator commands: either motion queue milestones, PID setpoints, PID velocities, or torques Sensor data: joint positions, joint velocities, accelerometers, force/torque… Controller

Important: Distinguishing model from simulator • A world model, and everything in it including the robot model, is NOT directly coupled with a simulation • i. e. , when you call Robot. Model. set. Config, it does NOT affect the simulation • The Controller is the only interface your robot’s “brain” has to interface with the simulation • The simulation stands in for the real world, and hence your Controller code should work similarly when put on a real robot • Caveat: the Simulator allows the programmer to observe the true state of the world & actuate things that don’t happen due to the robot’s control, e. g. , move objects, apply external forces, etc. • This information not realistic for a controller to have, and outside of early-stage prototyping you should not access Simulator methods in your control loop.

Visualization / user interaction GUI Simulator Actuator commands Sensor data Controller World model

Planning in the controller Simulator Actuator commands Sensor data World model Controller Planning (IK, dynamics, motion planning, etc. )

Ideally: Simulation is a stand-in for reality Real robot / world Simulator Sensor data Actuator commands World model Controller Planning (IK, dynamics, motion planning, etc. )

For quick prototyping… Usual pipeline: 1. Early stages: assume perfect knowledge of world 2. Accommodate imperfect knowledge in later stages GUI Simulator Actuator commands Sensor data Shared World model Controller Planning (IK, dynamics, motion planning, etc. )

Python Actuation API (Sim. Robot. Controller) • controller = sim. get. Controller(Robot. Model or robot index) • Setup: • • controller. set. PIDGains (k. P, k. I, k. D): overrides the PID gains in the Robot. Model to k. P, k. I, k. D (lists of floats of lengths robot. num. Drivers()) controller. set. Rate(dt): sets the time step of the internal controller to update every dt seconds • Basic low-level commands: • • • controller. set. PIDCommand(qdes, [dqes ]): sets the desired PID setpoint controller. set. Velocity(dqdes, duration ): sets a linearly increasing PID setpoint for all joints, starting at the current setpoint, and slopes in the list dqdes. After duration time it will stop. controller. set. Torque(t): sets a constant torque command t, which is a list of n floats. • Motion queue (wraps around a PID controller): • • • Convention: set. X methods move immediately to the indicated milestone, add/append creates a motion from the end of the motion queue to the indicated milestone controller. remaining. Time(): returns the remaining time in the motion queue, in seconds. controller. set/add. Milestone(qdes, [dqdes]): sets/appends a smooth motion to the configuration qdes, ending with optional joint velocities dqdes. controller. add. Milestone. Linear(qdes): same as add. Milestone, except the motion is constrained to a linear joint space path (Note: add. Milestone may deviate) controller. set/append. Linear(qdes, dt): sets/appends a linear interpolation to the destination qdes, finishing in dt seconds controller. set/add. Cubic(qdes, dt): moves immediately along a smooth cubic path to the destination qdes with velocity dqdes, finishing in dt seconds

Motion Queue Motion queue controllers Current time PID Controller Torque Robot

Parameters: Trajectory suffix y(t): [0, T]->R Motion Queue Append trajectory Current time PID Controller Torque Robot

Motion Queue Current time PID Controller Torque Robot

Parameters: Insertion time tinsert Trajectory suffix y(t): [0, T]->R Motion Queue Insert trajectory Current time PID Controller Torque Robot

Motion Queue Current time PID Controller Torque Robot

Python Sensing API (Sim. Robot. Controller) • controller. get. Commanded. Config(): retrieve PID setpoint • controller. get. Commanded. Velocity(): retrieve PID desired velocity • controller. get. Sensed. Config(): retrieve sensed configuration from joint encoders • controller. get. Sensed. Velocity(): retrieve sensed velocity from joint encoders • controller. get[Named]Sensor(index or name): retrieve Sim. Robot. Sensor by index/name

Python Sensing API (Sim. Robot. Sensor) • sensor = controller. get[Named]Sensor(index or name) • sensor. name(): gets the sensor’s name string • sensor. type(): gets the sensor’s type string, can be • • • Joint. Position. Sensor Joint. Velocity. Sensor Driver. Torque. Sensor Contact. Sensor Force. Torque. Sensor Accelerometer Tilt. Sensor Gyro. Sensor IMUSensor Filtered. Sensor • sensor. measurement. Names(): returns a list of strings naming the sensor’s measurements • sensor. get. Measurements(): returns a list of floats giving the sensor’s measurements at the current time step

Implementing a higher-level controller Basic loop: 1. Initialize simulation and controller 2. Repeat: 1. 2. 3. 4. Read sensing from Sim. Robot. Controller Calculate actuation commands Send commands to Sim. Robot. Controller Advance simulation by time dt control_loop() function in examples

Emulating a real robot • Your physical robot will have its own API for controlling it which defines its sensors and control interface. • Manufacturer will provide CAD models, kinematics, and (if you are lucky) factory calibration parameters 1. Generate a robot file with appropriate geometry, kinematics, dynamics, and motor emulation parameters 2. Set up the correct sensors in XML format under <sensors> tag. Either 1. Tag a robot’s. rob file with “property sensors [file]” (see 2. 3. Klampt/data/robots/huboplus/sensors. xml for an example) Put the XML in a robot’s. urdf file under the <robot><klampt><sensors> tag Put the XML under the <simulator><robot><sensors> tag (see Klampt/data/sim_test_worlds/sensortest. xml for an example) 3. Restrict your controller to use the correct interface to the robot’s motors. (Recommend writing an interface layer that forces your controller to use the same control functionality)

Python API (Simulator) Setup and Advancing • sim = Simulator(world): creates a simulator for a given World. Model (note: cannot modify the world at this point) • sim. get. World(): retrieves the simulation’s World. Model • sim. update. World(): updates the World. Model to reflect the current state of the simulator • simulate(dt): advances the simulation by time dt (in seconds) • sim. fake. Simulate(dt): fake-simulates. Useful for fast prototyping of controllers • sim. get. Time(): returns the accumulated simulation time • sim. get. State(): returns a string encoding the simulation state • sim. set. State(state): sets the simulation state given the result from a previous get. State() call • sim. reset(): reverts the simulation back to the initial state • sim. set. Gravity(g): sets the gravity to the 3 -tuple g (default (0, 0, -9. 8)) • sim. set. Sim. Step(dt): sets the internal simulation time step to dt. If simulate() is called with a larger value dt’, then the simulation will integrate physics forward over several substeps of length at most dt

Python API: Artificial control of rigid bodies (Sim. Body) • • • [NOTE: reference frame is centered at center of mass] body = sim. body([Robot. Link. Model or Rigid. Object. Model]) body. get. ID(): retrieves integer ID of associated object in world body. enable(enabled=True)/is. Enabled(): pass False to disable simulation of the body. enable. Dynamics(enabled=True)/is. Dynamics. Enabled(): pass False to drive a body kinematically along a given path body. get. Transform()/set. Transform(R, t): gets/sets SE(3) element representing transform of body coordinates w. r. t. world body. get. Velocity()/set. Velocity(w, v): gets/sets the angular velocity w and translational velocity v of the body coordinates w. r. t. world body. get. Surface()/set. Surface(Surface. Parameters): gets/sets the body’s surface parameters body. get. Collision. Padding()/set. Collision. Padding(m): gets/sets the body’s collision margin (nonzero yields more robust collision handling) body. apply. Force. At. Point(fw, pw), apply. Force. At. Local. Point(fw, pl): adds a world-space force fw to a point, either pw in world coordinates or pl in body coordinates. Applied over duration of next Simulator. simulate() call body. apply. Wrench(f, t): adds a force f at COM and torque t over the duration of te next Simulator. simulate() call

Python API: Introspection of contact forces • sim = Simulator(…) • sim. enable. Contact. Feedback. All(): turns on contact feedback for all objects • sim. enable. Contact. Feedback(id 1, id 2): turns on contact feedback for contacts between objects with id’s id 1 and id 2 • sim. in. Contact/had. Contact(id 1, id 2): returns True if objects id 1 and id 2 are in contact at the end of the time step / had contact during the prior time step • sim. had. Penetration/had. Separation(id 1, id 2): returns True if objects id 1 and id 2 penetrated / were separated at any point during the prior time step • sim. get. Contacts(id 1, id 2): returns a list of contacts between id 1 and id 2 on the current time step. Each contact is a 7 -list [px, py, pz, nx, ny, nz, k. Friction] • sim. get. Contact. Forces(id 1, id 2): returns a list of contact forces, one for each of the contacts in sim. get. Contacts(id 1, id 2) • sim. contact. Force/contact. Torque(id 1, id 2): returns the contact force / torque at the end of last time step • Sim. mean. Contact. Force(id 1, id 2): returns the mean contact force over the entire last time step • from model import contact; contact. sim. Contact. Map(sim): returns a map from (id 1, id 2) pairs to contact. Contact. Point objects.