Dynamics, Simulation, and Control in Klamp’t

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

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

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

4. Summary • Dynamics functionality: • Performing forward / inverse dynamics • at a particular state • Evaluating terms of the standard dynamics equation • at a particular state • Constrained forward / inverse dynamics • Contact analysis, e.g. determining stability, force closure, joint torques, etc. • for a known contact formation consisting of point contacts • Simulation functionality: • Rigid body physics simulation • Emulation of PID- or torque-controlled motors • Emulation of sensors: joint encoders, accelerometers, contact sensors, force/torque sensors • Robust mesh-mesh contacts, point cloud contacts • State save/resume • Contact introspection

5. Concept #1: Robot model dynamic parameters • Each robot link has inertial parameters • Mass (scalar, > 0) • COM (R3, given in local coordinates) • Inertia matrix (M3x3, 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)

6. Dynamics • Can compute COM of robot overall • Standard form • Can compute from (forward dynamics) • Can compute from • Can compute , , , (arbitrary gravity vector) • Newton-Euler and Lagrangian forms, give same results • Basic calling convention: • Set the robot model’s state including BOTH and • Call the function with all remaining inputs to retrieve output

7. Constrained Dynamics • Solves subject to • for all in (fixed links) • for all (fixed joints) • for some matrix A and vector b (arbitrary constraints) • Can compute from (forward constrained dynamics) • Can compute from (inverse constrained dynamics) • Can compute linearization of dynamics • Can also retrieve constraint forces • Complexity: O(n3+m3), with m=# of constraints • C++ API only

8. Python API (RobotModelLink, Mass) • RobotLink.getMass(): returns the link’s Mass structure • RobotLink.setMass(mass): sets the link’s Mass structure • Mass structure • get/setMass(mass): get/sets total mass to a float • get/setCom(com): get/sets center of mass to a float 3-tuple in link-local coordinates • getInertia(inertia): gets inertia as a 9-vector, indicating rows of the inertia matrix in link-local coordinates • setInertia(inertia) sets inertia, either a float, float 3-tuple, or float 9-tuple. If float, sets inertia matrix to HL=I3x3*inertia. If 3-tuple, sets matrix to HL=diag(inertia)

9. Python API (RobotModel) • robot.getVelocity](): returns the model’s Velocity as a list of floats with length numLinks() • robot.setVelocity(dq): sets the model’s Velocity to dqgiven as a list/tuple of floats with length numLinks() • robot.get[Velocity/Acceleration/Torque]Limits(): returns maximum of model’s absolute Velocity/acceleration/torque as a listof floats with length numLinks() • robot.set[Velocity/Acceleration/Torque]Limits(vmax): sets maximum absolute Velocity/acceleration/torque given a listof floats with length numLinks() • robot.getCom():returns robot’s center of mass as a 3-tuple of floats • robot.getMassMatrix():returns nxn array (n nested length-n lists) describing the mass matrix at the robot’s current Config • robot.getMassMatrixInv() returns nxn array (n nested length-n lists) describing the inverse mass matrix at the robot’s current Config • robot.getCoriolisForceMatrix() returns nxnarray (n nested length-n lists) describing the Coriolis force matrix such that at the robot’s current Config / Velocity • robot.getCoriolisForces(): returns the length-n list giving the Coriolis torques at the robot’s current Config / Velocity • robot.getGravityForces(gravity): returns the length-n list giving the generalized gravity G at the robot’s current Config, given the 3-tuple gravity vector (usually (0,0,-9.8)) • robot.torquesFromAccel(ddq): solves for the inverse dynamics, returning the length-n list of joint torques that would produce the acceleration ddq at the current Config/Velocity • robot.accelFromTorques(torques) : solves for the forward dynamics, returning the length-n list of joint accelerations produced by the torques torques at the current Config/Velocity

10. 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

11. 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

12. 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 RobotModel.setConfig, 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.

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

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

15. 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.)

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

17. Python Actuation API (SimRobotController) • controller = sim.getController(RobotModel or robot index) • Setup: • controller.setPIDGains(kP,kI,kD): overrides the PID gains in the RobotModel to kP,kI,kD (lists of floats of lengths robot.numDrivers()) • controller.setRate(dt): sets the time step of the internal controller to update every dtseconds • Basic low-level commands: • controller.setPIDCommand(qdes,[dqes]): sets the desired PID setpoint • controller.setVelocity(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.setTorque(t): sets a constant torque command t, which is a list of n floats. • Motion queue (wraps around a PID controller): • Convention: setX methods move immediately to the indicated milestone, add/append creates a motion from the end of the motion queue to the indicated milestone • controller.remainingTime(): returns the remaining time in the motion queue, in seconds. • controller.set/addMilestone(qdes,[dqdes]): sets/appends a smooth motion to the configuration qdes, ending with optional joint velocities dqdes. • controller.addMilestoneLinear(qdes): same as addMilestone, except the motion is constrained to a linear joint space path (Note: addMilestone may deviate) • controller.set/appendLinear(qdes,dt): sets/appends a linear interpolation to the destination qdes, finishing in dt seconds • controller.set/addCubic(qdes,dqdes,dt): moves immediately along a smooth cubic path to the destination qdes with velocity dqdes, finishing in dtseconds

18. Motion queue controllers Motion Queue Current time time Torque PID Controller Robot

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

20. Motion Queue Current time time Torque PID Controller Robot

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

22. Motion Queue Current time time Torque PID Controller Robot

23. Python Sensing API (SimRobotController) • controller.getCommandedConfig(): retrieve PID setpoint • controller.getCommandedVelocity(): retrieve PID desired velocity • controller.getSensedConfig(): retrieve sensed configuration from joint encoders • controller.getSensedVelocity(): retrieve sensed velocity from joint encoders • controller.get[Named]Sensor(index or name): retrieve SimRobotSensor by index/name

24. Python Sensing API (SimRobotSensor) • 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 • JointPositionSensor • JointVelocitySensor • DriverTorqueSensor • ContactSensor • ForceTorqueSensor • Accelerometer • TiltSensor • GyroSensor • IMUSensor • FilteredSensor • sensor.measurementNames(): returns a list of strings naming the sensor’s measurements • sensor.getMeasurements(): returns a list of floats giving the sensor’s measurements at the current time step

25. Implementing a higher-level controller Basic loop: • Initialize simulation and controller • Repeat: • Read sensing from SimRobotController • Calculate actuation commands • Send commands to SimRobotController • Advance simulation by time dt control_loop() function in examples

26. 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 • Generate a robot file with appropriate geometry, kinematics, dynamics, and motor emulation parameters • Set up the correct sensors in XML format under <sensors>tag. Either • Tag a robot’s .rob file with “property sensors [file]” (see 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) • 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)

27. Python API (Simulator) Setup and Advancing • sim = Simulator(world): creates a simulator for a given WorldModel (note: cannot modify the world at this point) • sim.getWorld(): retrieves the simulation’s WorldModel • sim.updateWorld(): updates the WorldModel to reflect the current state of the simulator • sim.simulate(dt): advances the simulation by time dt (in seconds) • sim.fakeSimulate(dt): fake-simulates. Useful for fast prototyping of controllers • sim.getTime(): returns the accumulated simulation time • sim.getState(): returns a string encoding the simulation state • sim.setState(state): sets the simulation state given the result from a previous getState() call • sim.reset(): reverts the simulation back to the initial state • sim.setGravity(g): sets the gravity to the 3-tuple g (default (0,0,-9.8)) • sim.setSimStep(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

28. Python API: Artificial control of rigid bodies (SimBody) • [NOTE: reference frame is centered at center of mass] • body = sim.body([RobotLinkModel or RigidObjectModel]) • body.getID(): retrieves integer ID of associated object in world • body.enable(enabled=True)/isEnabled(): pass False to disable simulation of the body • body.enableDynamics(enabled=True)/isDynamicsEnabled(): pass False to drive a body kinematically along a given path • body.getTransform()/setTransform(R,t): gets/sets SE(3) element representing transform of body coordinates w.r.t. world • body.getVelocity()/setVelocity(w,v): gets/sets the angular velocity w and translational velocity v of the body coordinates w.r.t. world • body.getSurface()/setSurface(SurfaceParameters): gets/sets the body’s surface parameters • body.getCollisionPadding()/setCollisionPadding(m): gets/sets the body’s collision margin (nonzero yields more robust collision handling) • body.applyForceAtPoint(fw,pw),applyForceAtLocalPoint(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.applyWrench(f,t): adds a force f at COM and torque t over the duration of te next Simulator.simulate() call

29. Python API: Introspection of contact forces • sim= Simulator(…) • sim.enableContactFeedbackAll(): turns on contact feedback for all objects • sim.enableContactFeedback(id1,id2): turns on contact feedback for contacts between objects with id’s id1 and id2 • sim.inContact/hadContact(id1,id2): returns True if objects id1 and id2 are in contact at the end of the time step / had contact during the prior time step • sim.hadPenetration/hadSeparation(id1,id2): returns True if objects id1 and id2 penetrated / were separated at any point during the prior time step • sim.getContacts(id1,id2): returns a list of contacts between id1 and id2 on the current time step. Each contact is a 7-list [px,py,pz,nx,ny,nz,kFriction] • sim.getContactForces(id1,id2): returns a list of contact forces, one for each of the contacts in sim.getContacts(id1,id2) • sim.contactForce/contactTorque(id1,id2): returns the contact force / torque at the end of last time step • Sim.meanContactForce(id1,id2): returns the mean contact force over the entire last time step • from model import contact; contact.simContactMap(sim): returns a map from (id1,id2) pairs to contact.ContactPoint objects.