Understanding Dynamics, Simulation, and Control in Klamp't
This document delves into the dynamics, simulation, and control aspects in Klamp't, covering key concepts, toolkit components, robot modeling, planning, 3D mathematics, and more. It explains how to perform forward/inverse dynamics, simulate rigid body physics, and emulate sensors. The content also discusses robot model dynamic parameters, computing center of mass, and constrained dynamics in detail.
Download Presentation
Please find below an Image/Link to download the presentation.
The content on the website is provided AS IS for your information and personal use only. It may not be sold, licensed, or shared on other websites without obtaining consent from the author. Download presentation by click this link. If you encounter any issues during the download, it is possible that the publisher has removed the file from their server.
E N D
Presentation Transcript
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 3D math & geometry Paths & trajectories Inverse Kinematics Motion planning Forward Kinematics Path Trajectory optimization Dynamics smoothing Contact mechanics Physics simulation Design tools Visualization System integration Motion design Robot posing ROS JSON / Sockets interface Physics simulation Path planning Disk I/O
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
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)
Dynamics Can compute COM of robot overall Standard form ? ? Can compute ? from ?,?,? (forward dynamics) Can compute ? from ?,?, ?(inverse dynamics) Can compute ?(?), ? 1(?), ?(?, ?), ?(?) (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
Constrained Dynamics Solves ? ? ? + ? ?, ? + ? ? = ? subject to ? ??(??(?)) = 0 for all ? in ?? 1, ,? (fixed links) ??= 0 for all ?? 1, ,? (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
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)
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 dq given 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 list of floats with length numLinks() robot.set[Velocity/Acceleration/Torque]Limits(vmax): sets maximum absolute Velocity/acceleration/torque given a list of 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 nxn array (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
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 RobotModel.setConfig, it does NOT affect the simulation The Controller is the onlyinterface 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 World model Actuator commands Sensor data Controller
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 Shared World model Sensor data Actuator commands Controller Planning (IK, dynamics, motion planning, etc.)
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 dt seconds 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 dt seconds
Motion queue controllers Motion Queue Current time time ????, ???? Torque PID Robot Controller ?, ?
Parameters: Trajectory suffix y(t):[0,T]->R Append trajectory Motion Queue Current time time Torque PID Robot Controller ?, ?
Motion Queue Current time time ????, ???? Torque PID Robot Controller ?, ?
Parameters: Insertion time tinsert Trajectory suffix y(t):[0,T]->R Insert trajectory Motion Queue Current time time Torque PID Robot Controller ?, ?
Motion Queue Current time time ????, ???? Torque PID Robot Controller ?, ?
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
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
Implementing a higher-level controller Basic loop: 1. Initialize simulation and controller 2. Repeat: 1. Read sensing from SimRobotController 2. Calculate actuation commands 3. Send commands to SimRobotController 4. 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 Klampt/data/robots/huboplus/sensors.xml for an example) 2. Put the XML in a robot s .urdf file under the <robot><klampt><sensors> tag 3. 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 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
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
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.
