Introduction to Klamp’t

1. Introduction to Klamp’t Kris Hauser ECE 383 / ME 442

2. Agenda • Intro to Klamp’t • Key concepts • World • Robot • Configuration • Link • IK Objective • IK Solver • Klamp’t Python API

3. What is Klamp’t? • A suite of software tools for planning and simulation of many-DOF robots • Planning: state-of-the-art • Simulation: state-of-the-art • Feedback control: some basic functionality • System integration: some tools for linking with ROS • Perception: nothing whatsoever • Intended audiences: primarily education & research • Cross platform: Linux, Mac, Windows • Languages: C++ and Python • Team • Lead: Kris Hauser • Contributors: Jordan Tritell, Jingru Luo, Alessio Rocchi • Mac build: Adam Konnecker, Cam Allen, Steve Kuznetsov

4. Historical context • Comparable packages: • OpenRAVE: probably most similar, not much legged robot support, simulation not as stable • ROS: is primarily a system integration tool, very steep learning curve esp. for non-CS types • With KDL/MoveIt!/Gazebo packages, can have similar functionality to Klamp’t • Robot simulators, e.g., V-REP, Webots, Gazebo: have more tools for behavior scripting, fewer tools for planning / analysis, simulation less stable • Klamp’t was developed from a historical code base (c. 2007), integrated with simulation in 2009, first release in 2014 • Started about the same time as OpenRAVE • To collect and reuse research products (and byproducts) • During DARPA Robotics Challenge, simulation was found much more stable than the dominant robot simulator, Gazebo, leading to first release

5. Design philosophy • Give students/researchers/engineers convenient programming tools for: • Learning robotics • Analyzing robots • Prototyping & developing intelligent robot behaviors • Once behaviors are developed in simulation, a user can apply them to their robot using ROS… or DIY system integration

6. Use in real-world projects • DARPA Robotics Challenge: DRC-Hubo walking / ladder climbing (Purdue, Indiana U, Drexel) • Amazon Picking Challenge, Team Duke (Duke, Prof. Hauser) • Compliant underactuated hand simulation (Stanford, IIT/Pisa, Columbia) • Planning-on-a-chip: precomputing probabilistic roadmaps (Brown, Duke)

7. Use in research • Boundary Layer Expanded Mesh (BLEM) contact generation for robust simulation (ISRR 2013) • Minimum Constraint Removal motion planning problems (WAFR 2012, IJRR 2014) • Fast trajectory time scaling via convex optimization and constraint projection (RSS 2013, ICRA 2014, IJRR 2014) • Motion planning and simulation testing of a ladder climbing humanoid (ICRA 2014) • Real time planning with application to smart teleoperation (RSS 2011, RSS 2012) / safety filtering for RoboPuppet miniature robot input devices (IROS 2014) • Planning for object throwing (ICRA 2012)

8. Toolkit Components 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

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

10. Python API • A wrapper around C++ API (bindings generated by SWIG) • Documentation: http://motion.pratt.duke.edu/klampt/pyklampt_docs/ • Some of it looks a little weird due to SWIG • All the core objects are in the robotsim module (and get loaded automatically into the klampt namespace upon “import klampt”) http://motion.pratt.duke.edu/klampt/pyklampt_docs/namespaceklampt_1_1robotsim.html • Newer, cleaner than C++ code. No need to worry about compilers. • A good place to start learning! • But… not as fully functional. Missing features: • Trajectory optimization • Advanced motion planning • Dynamic robot / object creation (must be loaded from disk) • Exotic IK types: sliding planar, sliding axis, rotation axis-only

11. Assumptions • You have read / understood the Klamp’t Python math tutorial http://motion.pratt.duke.edu/klampt/tutorial_math.html

12. Concept #1: World model • A World Model contains some number of entities, which can be of the types robot, robot link, rigid object, or terrain • Robots are articulated and possibly actuated objects containing multiple robot links • Robot links are individual links of a robot • Rigid objects can rotate and translate, no independent actuation • Terrains do not move • Each entity has • An index into the list of entities of that type • A unique ID# in the World • Consists of one (or more) bodies. Each body has a coordinate frame and usually some attached geometry and an appearance • A World Model can be drawn in OpenGL • Can be created dynamically or loaded from an XML file • Several examples in Klampt/data/

13. Python API (WorldModel) • world = WorldModel(): constructor • world.readFile([world file]): reads from XML doc • world.loadElement([robot, object or terrain file]): adds an element to the World. Accepts .rob, .urdf, .obj, or geometry files • world.num[Robots,RigidObjects,Terrains]():returns the number of elements of the given type • world.[robot,rigidObject,terrain](index): returns a reference to the index’th element of the given type • world.[robot,rigidObject,terrain](name): returns a reference to the element of the given type with name name (namemust be a str) • world.numIDs(): returns the number of elements. Unique IDs run from 0 to numIDs()-1. • world.geometry(id): returns the Geometry3D of the element with the given unique ID • world.appearance(id): returns the Appearance of the element with the given unique ID • world.drawGL(): renders the world in OpenGL

14. World file format • XML based format (documentation here)

15. Concept #2: Robot Model • A Robot Model has NL DOFs and NL Robot Links. • The set of all DOFs is the robot’s Configuration • Each DOF may also have a velocity. The set of all velocities is the robot’s Velocity • Each DOF has joint limits, velocity limits, acceleration limits, and torque limits • IMPORTANT: The configuration/velocity of a Robot Model do not directly correspond with those of a physical / simulated robot. They do not even have to respect the joint/velocity limits. Think of them as temporary variables to help you perform calculations.(Sending commands to a physical / simulated robot will be covered elsewhere!)

16. Python API (RobotModel) • robot = world.robot([name or index]): access Robot model reference • robot.getID(): gets the unique ID of the Robot model in the World model • robot.num[Links/Drivers]():returns the number of links or drivers (joints are abstracted away in the Python API) • robot.[link/driver](index): returns a reference to the index’thRobotModelLink / RobotModelDriver • robot.[link/driver](name): returns a reference to the RobotModelLink / RobotModelDriver with the given name (name must have type str) • robot.get[Config/Velocity](): returns the model’s Configuration/Velocity as a list of floats with length numLinks() • robot.setConfig(q): sets the model’s Configuration to q given as a list/tuple of floats with length numLinks(). Also, updates the forward kinematics of all Robot Links. • robot.setVelocity(dq): sets the model’s Velocity to dqgiven as a list/tuple of floats with length numLinks() • robot.get[Joint/Velocity/Acceleration]Limits(): returns minimum/maximum of model’s Configuration/Velocity/acceleration as a pair of lists of floats with length numLinks() • robot.set[Joint/Velocity/Acceleration]Limits(vmin,vmax): sets minimum/maximum Configuration/Velocity/acceleration given two lists of floats with length numLinks() • robot.drawGL(): renders the robot in OpenGL

17. Robot file format #1: .rob files • List of attributes giving link / joint / actuator properties

18. A more complex example: 6R+3 robot (KinovaJaco)

19. Robot file format #2: URDF files • Used throughout ROS • XML-based format • <klampt> tag provides additional configuration specs • More verbose than .rob

20. Concept #3: Robot link model • A Robot Link corresponds to one of the robot model’s DOFs • It holds all information about the link in its reference coordinate frame (name, index, parent, reference transform to parent, type of joint, joint axis, mass) • It also holds information regarding its current transform given the robot model’s current configuration (calculated via forward kinematics) • It also helps you calculate Jacobians, velocities, etc

21. Python API (RobotModelLink) Configuration-independent • link = robot.link([name or index]): access Robot Link reference • link.getID(): gets the unique ID of the Robot Link in the World model • link.getName():returns a string naming this link • link.getRobot():returns a RobotModel to which this link belongs • link.getIndex():returns the link index on the RobotModelfor this link • link.getParent(): returns the index of the link’s parent (-1 for no parent) • link.setParent(p): sets the index of the link’s parent(-1 for no parent) • link.getAxis(): returns a 3-tuple of the link’s rotation/translation axis in local coordinates • link.setAxis(axis): sets the link’s rotation/translation axis to the given 3-tuple, specifying its local coordinates • link.getParentTransform(): returns a pair (R,t) defining the reference coordinate transform of this link with respect to its parent (see klampt.so3 for the format of R) • link.setParentTransform(R,t): sets the reference coordinate transform of this link with respect to its parent (see klampt.so3 for the format of R) • link.geometry(): returns a reference to the Geometry3D attached to this link • link.appearance(): returns a reference to the Appearance attached to this link • link.getMass(): returns the link’s Mass structure • link.setMass(mass): sets the link’s Mass structure • link.drawLocalGL(): renders the link’s geometry in OpenGL in local coordinates

22. Python API (RobotModelLink) Configuration-dependent • link.getTransform(): returns a pair (R,t) defining the coordinate transform of this link with respect to the world frame (see klampt.so3 for the format of R) • link.setTransform(R,t): sets the coordinate transform of this link with respect to the world frame. Note: this does NOT perform inverse kinematics or change the transforms of any other links. The transform is overwritten when the robot's setConfig() method is called. (see klampt.so3 for the format of R) • link.getWorldPosition(pLocal): given a 3-tuple specifying the local coordinates of a point P, returns a 3-tuple giving the world coordinates of P. Equivalent to se3.apply(link.getTransform(),pLocal). • link.getLocalPosition(pWorld): given a 3-tuple specifying the local coordinates of a point P, returns a 3-tuple giving the world coordinates of P. Equivalent to se3.apply(se3.inv(link.getTransform()),pWorld). • link.getWorldDirection(dLocal): given a 3-tuple specifying the local coordinates of a direction D, returns a 3-tuple giving the world coordinates of D. Equivalent to so3.apply(link.getTransform()[0],dLocal). • link.getLocalDirection(dWorld): given a 3-tuple specifying the local coordinates of a point P, returns a 3-tuple giving the world coordinates of P. Equivalent to so3.apply(so3.inv(link.getTransform()[0]),dWorld). • link.getOrientationJacobian(): returns a 3xNLmatrix of the orientation Jacobian of this link • link.getPositionJacobian(): given a 3-tuple specifying the local coordinates of a point P, returns a 3xNL matrix of the position Jacobian of this point • link.getJacobian(p): given a 3-tuple specifying the local coordinates of a point P, returns a 6xNL matrix of the orientation Jacobian stacked on the position Jacobian • link.drawWorldGL(): renders the link’s geometry in OpenGL in world coordinates

23. Example • Load a planar 3R robot model and figure out where its end effector would lie at the configuration .

24. Code >>> from klampt import * >>> import math >>> world = WorldModel() >>> world.loadElement(“data/robots/planar3R.rob") Reading robot file robots/planar3R.rob... Parsing robot file, 3 links read... Loaded geometries in time 0.000598229s, 36 total primitive elements Initialized robot collision data structures in time 0.00084037s Done loading robot file robots/planar3R.rob. 0 >>> robot= world.robot(0) >>> robot.setConfig([0,math.pi/4,math.pi/4]) >>> link = robot.link(2) >>> link.getWorldPosition([1,0,0]) [1.7071067811865477, 0.0, 1.7071067811865475] >>> link.getPositionJacobian([1,0,0]) [[-1.7071067811865475, -1.7071067811865475, -1.0], [0.0, 0.0, 0.0], [1.7071067811865477, 0.7071067811865477, 1.5701957963021318e-16]]

25. Concept #4: Joints and Drivers • A Robot Model’s DOFs are organized into NJ<=NL Robot Joints, and ND<=NLRobot Drivers • Joint: topology of one or more DOFs • Driver: mapping from actuator controls to movement of one or more DOFs. • For fixed-base, fully actuated robots, each DOF has a single joint and a single driver: NL=NJ=ND • Joints specify how to handle interpolation, distance between configurations (welded, standard, freely rotating, or free floating) • Drivers specify how actuator commands are transmitted to forces / torques on links (e.g., the transmission)

26. Example

27. Concept #5: IK objectives • An IK Objective defines a target in Cartesian world space coordinates that you want to achieve with a link of the robot • There are also relative IK Objectives that let you define a constraint from one link to another on the same robot… used less frequently • Multiple links can be constrained to different targets using a list of IK Objectives • Contains • A link index • For relative objectives, a destination link index • A position constraint type: none, planar, linear, fixed • For non-none constraints, a local position and a target position • For planar and linear constraints, a direction of the plane/line • A rotation constraint type: none, axial, fixed • For non-none constraints, a target rotation • For axial constraints, a rotation axis • They ask the IK solver to find a configuration so that transformation by Tlink(q) matches the specified local coordinates to the specified target coordinates

28. Usual IK types • The target position is moved • The solver moves the configuration to keep the local point mapped to the target • Orientation is not constrained • Point constraint • Position: fixed • Rotation: none • 3D constraint • Fixed constraint • Position: fixed • Rotation: fixed • 6D constraint • The target position is moved • Position of point is moved keeping orientation fixed • The target rotation is changed • Position of point is fixed while orientation changes

29. Usual IK types • Hinge constraint • Position: fixed • Rotation: axis • Useful for grasping cylindrical objects • Surface constraint • Position: planar • Rotation: axis • Useful for placing objects down with unspecified position

30. Python API (IKObjective, ik module) Constructors • obj = ik.objective(body, ref=None, local=lplist, world=wplist): creates an IKObjective that constrains some local points on body (usually a RobotModelLink) to world points. lplist is either a single 3d point or a list of 3d points. wplisttaks the same format as lplist, and if it is a list of points, it needs to be of the same length. • obj = ik.objective(body, ref=None, R=R, t=t) : creates an IKObjective that constrains a body (usually a RobotModelLink) so its frame has a fixed rotation R and translation t. • obj = IKObjective(): manual constructor • obj.setFixedPoint(link,lp,wp): creates a point constraint on the link indexed by link. lp and wp are the 3D local and world points. • obj.setFixedPoints(link,lplist,wplist): creates a point constraint on the link indexed by link. lplistand wplistare lists of local and world points. • obj.setFixedTransform(link,R,t): creates a fixed constraint on the link indexed by link so its frame has the fixed rotation R and translation t.

31. Python API (IKObjective, ik module) Accessors • obj.num[Pos/Rot]Dims(): returns the number of positions of position / orientation constrained (0-3) • obj.getPosition(): returns a pair (local,world) of the constrained position. For fixed constraints local is the origin [0,0,0] and world is the target point. • obj.getPositionDirection(): if the position has a planar constraint (numPosDims()=1), the normal direction of the plane. If this is an linear constraint (numPosDims()=2), the direction of the line • obj.getRotation(): if the rotation is fixed (numRotDims()=3), the rotation matrix R. • obj.getRotationAxis(): if the rotation is axial (numRotDims()=2) returns a pair (laxis,waxis) giving the local / world coordinates of the constrained axis. • obj.getTransform(): if this is a fixed constraint, returns the pair (R,t) giving the fixed transform of the link

32. Concept #6: IK Solver • Klamp’t has a numerical IK solver • Newton-Raphson, with line search (never diverges) • Optional joint limits • Input • Robot model • One or more IK objectives • Seed configuration: model’s current configuration • Tolerance on max constraint error • Maximum iteration count • Optional: sub-select active DOFs (default uses all ancestors of constrained links), or custom joint limits • Output: • Success or failure (i.e. did not achieve desired tolerance) • Solution configuration returned as Robot model’s configuration

33. Python API (IKSolver, ik module) • ik.solve(objectives,iters=1000,tol=1e-3): Solves one or more IK objectives with the given max iteration count iters and constraint tolerance tol. Returns True if successful. Seeded by the robot’s current configuration, and on output the robot is set to the best found configuration. • solver = ik.solver(objectives): creates a solver for the given (one or more) objectives. • solver = IKSolver(robot): creates a solver for the given robot model. • solver.add(objective): adds another IKObjective to the solver. • solver.setActiveDofs(dofs): sets the active DOFs, given as a list of integer indices (default: all ancestor links of the constrained links). • solver.getActiveDofs(): gets the active DOFs as a list of integer indices. • solver.setJointLimits(qmin,qmax): sets custom joint limits, each a list of NL limits (default: solver uses the robot model’s joint limits). • solver.sampleInitial(): generates a random configuration as the seed • solver.solve(iters,tol): solves for the current set of IK objectives (arguments same as ik.solve). Returns (success,iters), where iters is the number of iterations used. • solver.getJacobian()/ik.jacobian(objectives): returns the matrix of IK objective derivatives with respect to the active DOFs. • solver.getResidual()/ik.residual(objectives): returns the vector of IK objective values at the robot’s current configuration

34. Example • Find a configuration where the end effector of a planar 3R robot touches the point (1.5,1)

35. Code >>> from klampt import * >>> from klampt import ik >>> world = WorldModel() >>> world.loadElement(“data/robots/planar3R.rob") … >>> robot= world.robot(0) >>> link = robot.getLink(2) >>> print robot.getConfig() [0.0, 0.0, 0.0] >>> obj = ik.objective(link,local=[1,0,0],world=[1.5,0,1]) >>> solver = ik.solver(obj) >>> solver.solve(100) True >>> print robot.getConfig() [0.9280844225663805, 5.24982420453923, 2.3118916002271988] >>> print solver.getResidual() [-4.36569416761845e-06, 0.0, -2.3191920574427982e-05]

36. Code >>> obj2 = ik.objective(link,local=[1,0,0],world=[3,0,1.5]) >>> solver = ik.solver(obj2) >>> solver.solve(100) (False, 6) >>> print robot.getConfig() [0.43734637942594445, 0.0047157337713716885, 6.278796895032342] >>> print solver.getResidual() [-0.2845097603140254, 0.0, -0.22482504933765957] >>> print robot.getLink(2).getWorldPosition([1,0,0]) [2.7154902396859746, 0.0, 1.2751749506623404] >>> print solver.getJacobian() [[-1.2751749506623402, -0.8516378500751175, -0.423833591599536], [0.0, 0.0, 0.0], [2.7154902396859737, 1.809611481558724, 0.9057400767504095]]

37. Klamp’t TODO • More advanced planning functionality (goal sets, optimization functions, planner diagnosis) • Unified app with built-in world design / planning / optimization / simulation • Massive parallel simulation • Visualizer customization in GUI • Tighter integration with Jupyter Notebooks (saving state, simulations, movies, interactive clicking) • Developers wanted

38. Next time • Movement along trajectories • Reading: • RS Chapter 15.1-2

39. Apps (Desktop version) • RobotTest: debug a robot model • RobotPose: pose a robot, edit motions and constraints. Load/save resources to disk • SimTest: simulate a robot • Utilities: • SimUtil: simulate a robot (command line) • TrajOpt: create an optimized trajectory along a geometric path • MotorCalibrate: calibrate the motor parameters of a robot model, given recorded data from real robot • Merge: combine multiple robots / rigid objects into a “mega robot” • Could these be unified? Yes, that would be nice! It just takes a SW developer’s time… Most frequently used

40. Python API (IKObjective, ik module) • obj= ik.fixed_objective(body,ref=None,local=lplist,world=wplist): creates an IKObjective that constrains some local points on body (usually a RobotModelLink) to world points. lplist is either a single 3d point or a list of 3d points. wplisttaks the same format as lplist, and if it is a list of points, it needs to be of the same length. • ik.solve_global(objectives, iters=1000, tol=1e-3, numRestarts=100, feasibilityCheck=None,startRandom=False): a global IK solver using random restarts + Newton Raphson. Also accepts a feasibility checker. • ik.solve_nearby(objectives, maxDeviation, iters=1000, tol=1e-3, numRestarts=0, feasibilityCheck=None): an IK solver that does not permit the current configuration to move more than a certain amount. Useful for incremental IK objectives, e.g., to generate movement along a Cartesian path.