klampt.txt

-------------------------------------------------------------------------------
MODELING
-------------------------------------------------------------------------------
  A container for all the entities which we use
  Robot, Terrain, RigidObject, MultiPath
  EntryFile: World.h
  
  Note: 
    Robot corresponds to the "desired" yellowish robot in simulation; to put
    the real robot exactly at a position can be done over ODERobot
    WorldSimulation->GetRobot(). This is only for starting purposes of course.
    Control acts on ODEWorldSimulationRobot, while all the kinematic
    solutions/setconfig act on Yellow Robot. 
    Be sure to update the frames or use UpdateConfig to set the robot
-------------------------------------------------------------------------------

RobotWorld (World.h) contains 
  Robot, Terrain, RigidObject, MultiPath!
  Names, Ids
  Load/Save XML

World -> Terrain -> ManagedGeometry -> DrawGL()
-> GLDraw::GeometryAppearance -> DrawGL() ->
  
RobotKinematics3D:
  ///for a force f at pi on link i, returns joint torques F = J^t f
  void GetForceTorques(const Vector3& f, const Vector3& pi, int i, Vector& F) const;

  Jacobian:
  void GetCOMJacobian(Matrix& Jc) const;
  void GetCOMHessian(Matrix& Hx,Matrix& Hy,Matrix& Hz) const;

  void GetDriverJacobian(int driver,Vector& J);

  /// Computes the time derivative of dpi/dqj.
  /// That is, the jacobian of pi on link i with respect to qj.
  bool GetJacobianDt(const Vector3& pi, int i, int j, Vector3&dtheta_dt,Vector3& dp_dt) const;
  ///gets the jacobian of pi w.r.t qj
  bool GetJacobian(const Vector3& pi, int i, int j, Vector3& dw, Vector3& dv) const;
  bool GetOrientationJacobian(int i, int j, Vector3& dw) const;
  bool GetPositionJacobian(const Vector3& pi, int i, int j, Vector3& dv) const;
  ///gets the jacobian matrix of pi w.r.t q
  ///row 0-2 are angular, 3-5 are translational
  void GetFullJacobian(const Vector3& pi, int i, Matrix& J) const;
  ///rows 3-5 of the above
  void GetPositionJacobian(const Vector3& pi, int i, Matrix& J) const;


struct RobotJointDriver
  enum Type { Normal, Affine, Translation, Rotation, Custom };
  int NumControls() const;  //number of input controls
  int NumLinks() const;
  Type type;
  vector<int> linkIndices;
  Real qmin,qmax;           //min/max values
  Real vmin,vmax;           //min/max velocities
  Real amin,amax;           //min/max accelerations
  Real tmin,tmax;           //min/max torques
  Real servoP,servoI,servoD;  //servo parameters
  Real dryFriction;           //constant friction coefficient
  Real viscousFriction;       //velocity-dependent friction coefficient

class RobotLink3D
  ///links's transformation is a function of control qi
  ///say we're link i
  ///T(i->i)(qi) = R(w*qi)oT(v*qi) where R rotates about an axis, T translates
  ///T(i->pi)(qi) = T0(i->pi)*T(i->i)(qi)
  void GetLocalTransform(Real qi,Frame3D& T) const;

  ///velocity of a point (in frame 0) with respect to qi,dqi
  void GetVelocity(Real qi,Real dqi,const Vector3& p,Vector3& vel) const;
  void GetAngularVelocity(Real dqi,Vector3& omega) const;

  ///Jacobian (orientation,position) of a point (in frame 0) with respect to qi
  void GetJacobian(Real qi,const Vector3& p,Vector3& Jo,Vector3& Jp) const;
  void GetOrientationJacobian(Vector3& Jo) const;
  void GetPositionJacobian(Real qi,const Vector3& p,Vector3& Jp) const;

  //Position jacobian of points in frame 0 w.r.t. i 
  void GetJacobian(Real qi,Frame3D& J) const; 
  //Position jacobian of points in frame j w.r.t. i
  void GetJacobian(Real qi,const Frame3D& Tj_World,Frame3D& J) const; 
  void GetWorldInertia(Matrix3& inertiaWorld) const;
  void GetWorldCOM(Vector3& comWorld) const { T_World.mulPoint(com,comWorld); }

  Type type; /// Indicates the type of joint- revolute, prismatic
  Vector3 w; /// The axis of rotation/translation (in local frame)
  Real mass;
  Vector3 com; /// The center of mass (in local frame)
  Matrix3 inertia; /// The inertia matrix (in local frame)
  /// Temporary - holds the current state of local to world transformation
  Frame3D T_World;


Robot (Modeling/Robot.h)
bool InJointLimits(const Config& q) const;
virtual bool SelfCollision(Real distance=0);


  Robot : public RobotWithGeometry
    string name;
    vector<string> geomFiles;   ///< geometry file names (used in saving)
    vector<ManagedGeometry> geomManagers; ///< geometry loaders (speeds up loading)
    Vector accMax;   ///< conservative acceleration limits, used by DynamicPath
    vector<RobotJoint> joints;
    vector<RobotJointDriver> drivers;
    vector<string> linkNames;
    vector<string> driverNames;

  <- RobotWithGeometry

    SelfCollision()
    DrawGL()
    DrawLinkGL(int i)
    int LinkIndex(char *) GetBody|GetLink function
    virtual bool SelfCollision(Real distance=0);


  <- <- RobotDynamics3D 

    Vector dq;   ///< current velocity
    Vector velMin,velMax; ///< velocity limits
    Vector torqueMax;     ///< torque limits
    Vector powerMax;      ///< Power=|torque||velocity| limits

    //B*ddq + C*dq = fext
      //ddq from fext
      void CalcAcceleration(Vector& ddq, const Vector& fext);
      //fext from ddq
      void CalcTorques(const Vector& ddq, Vector& fext);


  <- <- <- RobotKinematics3D 

    Vector3 GetCOM()
    GetTotalMass()
    std::vector<RobotLink3D> links;
    Config q;  //! DO NOT set directly, use UpdateConfig!
    Vector qMin,qMax;   ///< joint limits

    /// sets the current config q and updates frames
    void UpdateConfig(const Config& q);
    /// based on the values in q, update the frames T
    void UpdateFrames();
    bool InJointLimits(const Config& q) const;

    //Force Field acts on rigid link i and induces a wrench on its COM
    // wrench on COM of link i induces a wrench on CS of robot. 
    // F = J^t w | w=(torque,force)
    void GetWrenchTorques(const Vector3& torque, const Vector3& force, int i, Vector& F) const;


  <- <- <- <- Chain
    //on-chain/off-chain idea goes here:
    void GetChildList(std::vector<std::vector<int> >& children) const;


RobotLink3D (KrisLibrary/robotics/RobotLink3d.h
  Type type; {revolute,prismatic}
  Vector3 w;
  Real mass;
  Vector3 com;
  Matrix3 inertia;


AnyCollisionQuery: KrisLibrary/geometry/AnyGeometry.cpp

  //extracts the pairs of interacting points on a previous call to
  //WithinDistance[All], PenetrationDepth, or Distance.  Points are given
  //in local frames.
  void InteractingPoints(std::vector<Vector3>& p1,std::vector<Vector3>& p2)
const;
bool CheckCollision(AnyCollisionGeometry3D* m1,AnyCollisionGeometry3D* m2,Real
tol)
{
  if(!m1 || !m2) return false;
  Assert(tol >= 0);
  AnyCollisionQuery q(*m1,*m2);
  if(tol == 0) {
    return q.Collide();
  }
  else {
    return q.WithinDistance(tol);
  }
}




-------------------------------------------------------------------------------
COMMON ERRORS:
-------------------------------------------------------------------------------
glError GL_INVALID_OPERATION found at /home/aorthey/git/Klampt/Interface/NavigationGUI.cpp:203
(means we have setlinewidth inside an opengl operation)

-------------------------------------------------------------------------------
INTERFACE
-------------------------------------------------------------------------------

  Controls the communication between frontend and backend
  Frontend  (The GUI, viewer, interactions, dynamic simulation, drawGL)
    GenericGUIBase (GenericGUI.h)
  Backend (The world and its entities)
    GenericBackendBase (GenericGUI.h)

-------------------------------------------------------------------------------
BACKEND [Interface/*GUI.h]
-------------------------------------------------------------------------------
  GenericBackendBase [GenericGUI.h] 
    bool GenericGUIBase::SendGLViewport(int x,int y,int w,int h)

  <- MouseDragBackend [NavigationGUI.h] 
  <- <- GLNavigationBackend [NavigationGUI.h] 
          Camera::Viewport viewport; viewport.x .y .w .h 
          OnGLRender()
            RenderWorld()

  <- <- <- WorldGUIBackend [WorldGUI.h] (loader for RobotWorld)
              RenderWorld()
                drawCoords(0.1);

  <- <- <- <- SimGUIBackend [SimulationGUI.h] (add functions to display dynamics,i.e. wrenches, torques, contacts etc)
                RenderWorld()
                  world->terrains[i]->DrawGL();
                  world->rigidObjects[i]->DrawGL();
                  world->robotViews[i].DrawLink_World(j);

  <- <- <- <- <- SimTestBackend [SimTestGUI.h] (more functions to interact with robot, add forces, etc)
                  RenderWorld()
                    world->robotViews[r].SetColors(desiredColor);
                    world->robotViews[r].Draw();
                    world->robotViews[r].SetAppearance(oldAppearance);

-------------------------------------------------------------------------------
GUI
-------------------------------------------------------------------------------
  GLUIProgramBase [KrisLibrary/GLdraw/GLUIProgram.h]
  Run()
  int Run(const char *window_title="OpenGL Viewer",unsigned int displayMode=0);

  <- GLUIGUI : GenericGUIBase, GLUIProgramBase [Interface/GLUIGUI.h]

  <- <- GLScreenshotProgram<BaseGUI> : public BaseGUI

  <- <- <- GLUISimTestGUI : GLScreenshotProgram<GLUIGUI>
          Initialize()








adding new widgets: 
Creating button: 
Connecting button: button has to be connected with render drawing method => MapButtonToggle("draw_bbs",&drawBBs);


%%inline void DrawGLTris(const
%%                                                                                                              
%%{
%%  glBegin(GL_TRIANGLES);
%%  for(size_t i=0;i<mesh.tris.size();i++) {
%%  | Math3D::Vector3 normal =
%%  | glNormal3v(normal);
%%  | glVertex3v(mesh.verts[mesh.tris[i].a]);
%%  | glVertex3v(mesh.verts[mesh.tris[i].b]);
%%  | glVertex3v(mesh.verts[mesh.tris[i].c]);
%%  }
%%  glEnd();
%%}





-------------------------------------------------------------------------------
VIEW
-------------------------------------------------------------------------------
  drawing routines for all entities defined in RobotWorld
-------------------------------------------------------------------------------

Actual drawing happens in KrisLibrary/GLdraw/GeometryAppearance.cpp
KrisLibrary/GLdraw/drawextra.h

-------------------------------------------------------------------------------
SIMULATION
-------------------------------------------------------------------------------
  ode simulation files 
  everything related to the actual dynamical simulation
  EntryFile : WorldSimulation.h
-------------------------------------------------------------------------------

WorldSimulation
  double time

  /// anything contact forces/torques, where are the contacts etc goes here
  ODEContactList* GetContactList(int aid,int bid);
  Vector3 ContactForce(int aid,int bid=-1);

  // major players
  ODESimulator odesim; //your gateway to ODE geometrix
  RobotWorld *world;

  ODERobot GetRobot(int id); //get the real robot (you cannot and should not
    control that, but can be used for initial placement procedures)

-------------------------------------------------------------------------------
CONTACTS
-------------------------------------------------------------------------------

  Hold (a single robot link-env contact with 1 or more contact points)
  Stance (a set of holds)
  Grasp (a set of holds with some dofs fixed)

-------------------------------------------------------------------------------
CSPACE
-------------------------------------------------------------------------------

  CSpace (Krislibrary/planning/cspace.h)
    //Generic CSpace
    void Sample(Config& x);
    bool IsFeasible(const Config&);

  <- ExplicitCSpace (KrisLibrary/planning/ExplicitCspace.h)
    //N-obstacles CSpace
    bool IsFeasible(const Config&,int obstacle);
    EdgePlanner* LocalPlanner(const Config& a,const Config& b,int obstacle);
    int NumObstacles();
    void GetInfeasibleNames(const Config& q,std::vector<std::string>& names);

  <- <- SingleRobotCSpace  (Klampt/Planning/RobotCSpace.h)
    //Single Robot 
    vector<pair<int,int> > collisionPairs;
    bool CheckCollisionFree();
    WorldPlannerSettings* settings;
    RobotWorld& world;
    int index; ##robot number
    Robot* GetRobot&();


  <- <- <- <- SingleRobotCSpace2 (Klampt/Planning/RobotCSpace.h)
    //Single robot + fixed dofs available
    IgnoreCollisions(int,int)
    FixDof(int,double)

  <- <- <- <- <- ContactCSpace (Klampt/Planning/ContactCSpace.h)
    //Single Robot in contact with environment
    vector<IKGoal> contactIK;
    void AddContact(const IKGoal& goal);
    void RemoveContact(int link);
    bool SolveContact(int numIters=0,Real tol=0);

-------------------------------------------------------------------------------
COLLISIONS
-------------------------------------------------------------------------------
(Klampt/Planning/RobotCSpace.h)
-- SingleRobotCSpace  
  vector<pair<int,int> > collisionPairs;
  bool CheckCollisionFree();
  WorldPlannerSettings* settings;

(Klampt/Planning/PlannerSettings.h)
-- Worldplannersettings
CheckCollision(world, idrobot, idothers)
Real DistanceLowerBound(world, idrobot, idothers)

-------------------------------------------------------------------------------
KINODYNAMIC CSPACE
-------------------------------------------------------------------------------

  CSpace (Krislibrary/planning/cspace.h)
    void Sample(Config& x);
    bool IsFeasible(const Config&);

  <- KinodynamicCSpace (KrisLibrary/planning/KinodynamicCSpace.h)

  <- <- IntegratedKinodynamicCSpace : public KinodynamicCSpace


-------------------------------------------------------------------------------
MOTION PLANNER
-------------------------------------------------------------------------------

(KrisLibrary/planning/MotionPlanner.h)

RoadmapPlanner 
TreeRoadmapPlanner
  <- RRTPlanner
  <- <- BidirectionalRRTPlanner

(KrisLibrary/planning/AnyMotionPlanner.h)
  MotionPlannerFactory factory;
  factory.type="rrt"
  MotionPlannerInterface = factory.create(CSpace)

-------------------------------------------------------------------------------
LOAD/SAVE RESOURCE (KrisLibrary/utils/ResourceLibrary.h)
-------------------------------------------------------------------------------

  Modeling/Resources.h

    BasicResources (vector,transform,config)
    GraspResources

-------------------------------------------------------------------------------
MATH
 * The elements can be accessed as (i,j) for i,j in [0,3].

  WARNING: [i][j] = (j,i) !! DO NOT USE [i][j]
-------------------------------------------------------------------------------
 Infinity
  double dInf
  Real Inf

 Krislibrary/math3d/primitives.h
  class Vector2;
  class Vector3;
  class Vector4;
  class Matrix2;
  class Matrix3;
  class Matrix4;
  class RigidTransform2D;
  class RigidTransform;
  +IO functionality!

  - class Matrix3
    inline void setRotateX(Real rads);  ///<sets the matrix that rotates ccw by rads around the x axis
    inline void setRotateY(Real rads);  ///<sets the matrix that rotates ccw by rads around the x axis
    inline void setRotateZ(Real rads);  ///<sets the matrix that rotates ccw by rads around the x axis

  setIdentity()
  setZero()


 Krislibrary/math/Vectortemplate.h
  typedef class VectorTemplate<float> fVector;
  typedef class VectorTemplate<double> dVector;
  typedef class VectorTemplate<Complex> cVector;
  typedef VectorTemplate<Real> Vector;
    T dot(const MyT&) const;
    T dotSelf() const;
    T norm() const;
    T normSquared() const;
    T distance(const MyT&) const;
    T distanceSquared(const MyT&) const;
    T minElement(int* index=NULL) const;
    T maxElement(int* index=NULL) const;
    T minAbsElement(int* index=NULL) const;
    T maxAbsElement(int* index=NULL) const;

-------------------------------------------------------------------------------
PATH
-------------------------------------------------------------------------------

MilestonePath (KrisLibrary/planning/path.h)
  const Config& Start()
  const Config& End()
  bool IsFeasible();
  int Eval(Real t, Config& c) const;

MultiPath (Modeling/MultiPath.h)
  Load(string);
  Save(string);
  bool IsValid();
  Real StartTime() const;
  ///Returns the end time of the path.  If untimed, the end time is 1.
  Real EndTime() const;
  Real Duration() const;
  int Evaluate(Real time,Vector& q,Vector& v)

-------------------------------------------------------------------------------
Controller
-------------------------------------------------------------------------------

Control/Controller.h
-------------------------------------------------------------------------------
World Simulation
-------------------------------------------------------------------------------

 Every robot has an associated ControlledRobotSimulator class
(Simulation/ControllerSimulator.h)

It contains Robot, ODERobot, and RobotController. Also we can get a Sensor class
and set a Command Class:

  Robot* robot;
  ODERobot* oderobot;
  RobotController* controller;
  RobotMotorCommand command;
  RobotSensors sensors;

We need to reinit this one after changing the driver!