├── c++ └── PCG │ ├── src │ ├── Cylinder.h │ ├── Cylinder.cpp │ ├── JSONwrapper.h │ ├── RigidBodyUtility.h │ ├── JointFixed.h │ ├── StateDeriv.h │ ├── JointSlider.h │ ├── online │ │ ├── Brender │ │ │ └── cpp │ │ │ │ ├── BrenderManager.h │ │ │ │ ├── Brenderable.h │ │ │ │ └── BrenderManager.cpp │ │ ├── Program.h │ │ ├── GLSL.h │ │ ├── Camera.h │ │ ├── MatrixStack.h │ │ ├── Camera.cpp │ │ ├── MatrixStack.cpp │ │ ├── Program.cpp │ │ └── GLSL.cpp │ ├── JointHinge.h │ ├── JointBall.h │ ├── JointPrismatic.h │ ├── JointUniversal.h │ ├── JointPowered.h │ ├── JointSpringy.h │ ├── LinkageSystem.h │ ├── Rigid.h │ ├── Cuboid.h │ ├── Scene.h │ ├── ChronoTimer.h │ ├── JointBall.cpp │ ├── JointSlider.cpp │ ├── JointPrismatic.cpp │ ├── StateSolve.h │ ├── JointPowered.cpp │ ├── JointFixed.cpp │ ├── JointSpringy.cpp │ ├── JointHinge.cpp │ ├── Solver.h │ ├── JointUniversal.cpp │ ├── State.h │ └── RigidBodyCreator.h │ ├── resources │ ├── result_spline_auto_save.txt │ ├── simple_frag.glsl │ ├── simple_vert.glsl │ ├── splines_hand.txt │ ├── splines_auto_save.txt │ ├── linkages_auto_save.txt │ ├── cube.mtl │ ├── input_red.txt │ ├── phong_vert.glsl │ ├── input.txt │ ├── phong_frag.glsl │ ├── input_simple.txt │ ├── input_nospline.txt │ ├── input2.txt │ ├── input_test.txt │ ├── input_OPT.txt │ ├── input_OPT_123.txt │ ├── input_bad.txt │ ├── input_OPT_122.txt │ ├── cube.obj │ └── syntax.txt │ ├── README.md │ └── cmake │ ├── FindGLM.cmake │ ├── FindPardiso.cmake │ ├── FindMKL.cmake │ ├── FindGLEW.cmake │ ├── FindJSONCPP.cmake │ ├── FindGLFW.cmake │ └── FindEigen3.cmake ├── notes.pdf ├── matlab-diff └── +redmax │ ├── JointFixed.m │ ├── ForceNull.m │ ├── BodyCuboid.m │ ├── JointTranslational.m │ ├── JointPlanar.m │ ├── JointPrismatic.m │ ├── ForceSpringDamper.m │ ├── JointFree2D.m │ ├── ForceCable.m │ ├── Force.m │ ├── TaskBDF1.m │ ├── JointRevolute.m │ ├── TaskBDF2.m │ ├── TaskBDF1PointPos.m │ ├── TaskBDF2PointPos.m │ ├── JointFree3D.m │ └── JointUniversal.m ├── matlab ├── +redmax │ ├── SpringNull.m │ ├── DeformableNull.m │ ├── ConstraintNull.m │ ├── JointFixed.m │ ├── BodyCuboid.m │ ├── JointPlanar.m │ ├── JointTranslational.m │ ├── BodySphere.m │ ├── JointFree2D.m │ ├── ConstraintPrescJoint.m │ ├── ConstraintPrescBody.m │ ├── ConstraintMultQ.m │ ├── JointUniversal.m │ ├── JointFree3D.m │ ├── BodyCylinder.m │ ├── ConstraintJointLimit.m │ ├── ConstraintPrescJointM.m │ ├── JointComposite.m │ ├── ConstraintAttachSpring.m │ ├── JointFree.m │ ├── SpringPointDirection.m │ ├── Spring.m │ ├── BodyComposite.m │ ├── Deformable.m │ ├── JointPrismatic.m │ ├── ConstraintFloor.m │ ├── SpringPointPoint.m │ └── JointRevolute.m ├── cuboid.obj └── README.md ├── matlab-simple ├── +redmax │ ├── JointFixed.m │ ├── BodyCuboid.m │ ├── JointRevolute.m │ └── Scene.m ├── README.md ├── testRigid.m ├── testRedMax.m └── testRedMaxScenes.m ├── .gitignore ├── LICENSE └── README.md /c++/PCG/src/Cylinder.h: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /c++/PCG/src/Cylinder.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /c++/PCG/resources/result_spline_auto_save.txt: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /notes.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sueda/redmax/HEAD/notes.pdf -------------------------------------------------------------------------------- /c++/PCG/resources/simple_frag.glsl: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | varying vec3 color; 4 | 5 | void main() 6 | { 7 | gl_FragColor = vec4(color, 1.0); 8 | } 9 | -------------------------------------------------------------------------------- /c++/PCG/resources/simple_vert.glsl: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | uniform mat4 P; 4 | uniform mat4 MV; 5 | varying vec3 color; 6 | 7 | void main() 8 | { 9 | gl_Position = P * MV * gl_Vertex; 10 | color = gl_Color.rgb; 11 | } 12 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/JointFixed.m: -------------------------------------------------------------------------------- 1 | classdef JointFixed < redmax.Joint 2 | 3 | %% 4 | methods 5 | %% 6 | function this = JointFixed(parent,body) 7 | this = this@redmax.Joint(parent,body,0); 8 | end 9 | end 10 | end 11 | -------------------------------------------------------------------------------- /matlab/+redmax/SpringNull.m: -------------------------------------------------------------------------------- 1 | classdef SpringNull < redmax.Spring 2 | %SpringNull Empty spring, created if there are no other springs 3 | 4 | %% 5 | methods 6 | function this = SpringNull() 7 | this = this@redmax.Spring(); 8 | end 9 | end 10 | end 11 | -------------------------------------------------------------------------------- /c++/PCG/resources/splines_hand.txt: -------------------------------------------------------------------------------- 1 | -6 2.2 0 2 | -4 0 0 3 | 0 -0.8 0 4 | 5.5 0 0 5 | 9.5 3 0 6 | 5 2 0 7 | 0 0.8 0 8 | -5 2 0 9 | 10 | 11 | 12 | -4.0 0 0 13 | -2.828 -2.828 0 14 | 0 -4.0 0 15 | 2.828 -2.828 0 16 | 4.0 0 0 17 | 2.828 2.828 0 18 | 0 4.0 0 19 | -2.828 2.828 0 -------------------------------------------------------------------------------- /c++/PCG/resources/splines_auto_save.txt: -------------------------------------------------------------------------------- 1 | #spline 2 | prnt bottom 3 | type bspline 4 | ppos -3 0 0 5 | -3.68027 1.55635 0 6 | -5.07668 -0.773673 0 7 | 0 -0.8 0 8 | 4.89546 -0.943614 0 9 | 3.73454 1.77133 0 10 | 1.68198 2.4215 0 11 | 0.338043 3.00243 0 12 | -1.85995 3.29425 0 13 | -------------------------------------------------------------------------------- /matlab/+redmax/DeformableNull.m: -------------------------------------------------------------------------------- 1 | classdef DeformableNull < redmax.Deformable 2 | % DeformableNull Does nothing 3 | 4 | %% 5 | methods 6 | %% 7 | function this = DeformableNull() 8 | this = this@redmax.Deformable(); 9 | this.name = 'null'; 10 | end 11 | end 12 | end 13 | -------------------------------------------------------------------------------- /c++/PCG/resources/linkages_auto_save.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name top 3 | dens 3 4 | size 6 0.8 0.5 5 | prnt 6 | cpos 0 6.4 0 7 | ppos 1 1.5 0 8 | type ball 9 | angl 0.707 10 | #link 11 | name bottom 12 | dens 3 13 | size 6 0.8 0.5 14 | prnt top 15 | cpos 2.8 0 0 16 | ppos -2.8 0 0 17 | type ball 18 | -------------------------------------------------------------------------------- /matlab/+redmax/ConstraintNull.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintNull < redmax.Constraint 2 | % ConstraintNull Does nothing 3 | 4 | %% 5 | methods 6 | %% 7 | function this = ConstraintNull() 8 | this = this@redmax.Constraint(0,0,0,0); 9 | this.name = 'null'; 10 | end 11 | end 12 | end 13 | -------------------------------------------------------------------------------- /c++/PCG/resources/cube.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl Material 5 | Ns 96.078431 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.640000 0.640000 0.640000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/ForceNull.m: -------------------------------------------------------------------------------- 1 | classdef ForceNull < redmax.Force 2 | %ForceNull An empty force 3 | 4 | %% 5 | properties 6 | end 7 | 8 | methods 9 | function this = ForceNull() 10 | this = this@redmax.Force(); 11 | end 12 | end 13 | 14 | %% 15 | methods (Access = protected) 16 | 17 | end 18 | end 19 | -------------------------------------------------------------------------------- /matlab/+redmax/JointFixed.m: -------------------------------------------------------------------------------- 1 | classdef JointFixed < redmax.Joint 2 | 3 | %% 4 | methods 5 | %% 6 | function this = JointFixed(parent,body) 7 | this = this@redmax.Joint(parent,body,0); 8 | end 9 | end 10 | 11 | %% 12 | methods (Access = protected) 13 | %% 14 | function update_(this) 15 | this.Q = eye(4); 16 | end 17 | end 18 | end 19 | -------------------------------------------------------------------------------- /matlab-simple/+redmax/JointFixed.m: -------------------------------------------------------------------------------- 1 | classdef JointFixed < redmax.Joint 2 | 3 | %% 4 | methods 5 | %% 6 | function this = JointFixed(parent,body) 7 | this = this@redmax.Joint(parent,body,0); 8 | end 9 | end 10 | 11 | %% 12 | methods (Access = protected) 13 | %% 14 | function update_(this) 15 | this.Q = eye(4); 16 | end 17 | end 18 | end 19 | -------------------------------------------------------------------------------- /c++/PCG/resources/input_red.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name root 3 | dens 1 4 | size 8 8 0.5 5 | cpos 1.0 6.4 0.0 6 | invisible 7 | #link 8 | name top 9 | dens 3 10 | size 6 0.8 0.5 11 | prnt root 12 | cpos 1 1.5 0 13 | ppos 0 0 0 14 | angl .707 15 | type hinge 0 1 0 16 | #link 17 | name bottom 18 | dens 3 19 | size 6 0.8 0.5 20 | prnt top 21 | cpos 2.8 0 0 22 | ppos -2.8 0 0 23 | type hinge 0 1 0 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /c++/PCG/resources/phong_vert.glsl: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | attribute vec4 aPos; 4 | attribute vec3 aNor; 5 | attribute vec2 aTex; 6 | uniform mat4 P; 7 | uniform mat4 MV; 8 | varying vec3 vPos; 9 | varying vec3 vNor; 10 | varying vec2 vTex; 11 | 12 | void main() 13 | { 14 | vec4 posCam = MV * aPos; 15 | gl_Position = P * posCam; 16 | vPos = posCam.xyz; 17 | vNor = (MV * vec4(aNor, 0.0)).xyz; 18 | vTex = aTex; 19 | } 20 | -------------------------------------------------------------------------------- /c++/PCG/resources/input.txt: -------------------------------------------------------------------------------- 1 | #options 2 | simt redCGnomat 3 | #link 4 | name top 5 | dens 3 6 | size 3 0.8 0.5 7 | ppos 0 0 0 8 | cpos -1.4 0 0 9 | angl 1.707 10 | type hinge 0 0 1 11 | #link 12 | name middle 13 | dens 3 14 | size 3 0.8 0.5 15 | prnt top 16 | ppos 1.4 0 0 17 | cpos -1.4 0 0 18 | angl 0 19 | type hinge 0 0 1 20 | #spline 21 | prnt middle 22 | type bspline_open 23 | ppos -3.0 0 0 24 | -4 -4 0 25 | -3 -3 0 26 | -2 -2 0 27 | -1 -1 0 28 | 0 0 0 -------------------------------------------------------------------------------- /c++/PCG/src/JSONwrapper.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifdef REDMAX_JSONCPP 3 | #include 4 | #include 5 | #include 6 | 7 | struct JSONwrapper 8 | { 9 | Json::Value jsonscene; 10 | Json::Value frames; 11 | 12 | std::vector blocks; 13 | std::vector block_scales; 14 | std::vector block_locations; 15 | std::vector block_quats; 16 | 17 | JSONwrapper() 18 | { 19 | frames = Json::Value(Json::arrayValue); 20 | }; 21 | }; 22 | #endif 23 | -------------------------------------------------------------------------------- /matlab-simple/README.md: -------------------------------------------------------------------------------- 1 | We provide a reference implementation of the RedMax algorithm, written in object-oriented MATLAB (2018b). This code is not designed for performance but is rather designed for pedagogical purposes. To run the code, go to the directory containing testRedMax.m and type: 2 | 3 | ``` 4 | >> testRedMax(0) 5 | ``` 6 | This will show a swinging chain with alternating revolute/fixed joints (Fig. 1a). The argument switches the scene to be simulated: 7 | 8 | 0. Simple serial chain 9 | 1. Different revolute axes 10 | 2. Branching 11 | -------------------------------------------------------------------------------- /c++/PCG/resources/phong_frag.glsl: -------------------------------------------------------------------------------- 1 | #version 120 2 | 3 | varying vec3 vPos; // in camera space 4 | varying vec3 vNor; // in camera space 5 | varying vec2 vTex; 6 | uniform vec3 kdFront; 7 | uniform vec3 kdBack; 8 | 9 | void main() 10 | { 11 | vec3 lightPos = vec3(0.0, 0.0, 0.0); 12 | vec3 n = normalize(vNor); 13 | vec3 l = normalize(lightPos - vPos); 14 | vec3 v = -normalize(vPos); 15 | vec3 h = normalize(l + v); 16 | vec3 kd = kdFront; 17 | float ln = dot(l, n); 18 | if(ln < 0.0) { 19 | kd = kdBack; 20 | ln = -ln; 21 | } 22 | vec3 diffuse = ln * kd; 23 | vec3 color = diffuse; 24 | gl_FragColor = vec4(color, 1.0); 25 | } 26 | -------------------------------------------------------------------------------- /c++/PCG/src/RigidBodyUtility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define EIGEN_USE_MKL 4 | #define EIGEN_DONT_ALIGN_STATICALLY 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #define RB_M_PI 3.1415926535897932 11 | #define THRESHOLD 1e-10 12 | 13 | #define ONLINE_MODE 14 | 15 | static Eigen::Vector3d emptyVector3d; 16 | 17 | // Globlal Simulation Information 18 | enum simType { PCG, PCG_unopt, Pardiso}; 19 | extern std::set maximalCoordList; 20 | extern std::set reducedCoordList; 21 | extern std::set reducedNoMatrixList; 22 | 23 | // define the simulation method 24 | extern simType simtype; 25 | -------------------------------------------------------------------------------- /c++/PCG/resources/input_simple.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name root 3 | mass 1 4 | size 8 8 5 | cpos 1.0 6.4 0.0 6 | invisible 7 | #link 8 | name motor 9 | mass 0.25 10 | size 1 0.5 11 | prnt root 12 | cpos -0.25 0.0 0.0 13 | ppos 0 5 0 14 | angl 1 15 | type powered -10 16 | #link 17 | name top 18 | mass 3 19 | size 6 0.8 20 | prnt motor 21 | cpos 2.8 0 0 22 | ppos 0.75 0 0 23 | angl -0.5 24 | physics 25 | angl 0 26 | cpos 0 0 0 27 | #link 28 | name bottom 29 | mass 3 30 | size 6 0.8 31 | prnt top 32 | cpos 2.8 0 0 33 | ppos -2.8 0 0 34 | physics 35 | angl 0 36 | cpos 0 0 0 37 | #spline 38 | prnt bottom 39 | type bspline 40 | ppos -3.0 0 0 41 | 0 0 0 42 | 0 0 0 43 | 0 0 0 44 | 0 0 0 45 | 0 0 0 46 | 0 0 0 47 | 0 0 0 48 | 0 0 0 49 | 0 0 0 -------------------------------------------------------------------------------- /c++/PCG/resources/input_nospline.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name root 3 | mass 1 4 | size 8 8 5 | cpos 1.0 6.4 0.0 6 | invisible 7 | #link 8 | name motor 9 | mass 0.25 10 | size 1 0.5 11 | prnt root 12 | cpos -0.25 0.0 0.0 13 | ppos 0 5 0 14 | angl 1 15 | type powered -10 16 | #link 17 | name top 18 | mass 3 19 | size 6 0.8 20 | prnt motor 21 | cpos 2.8 0 0 22 | ppos 0.75 0 0 23 | angl -0.5 24 | physics 25 | angl 0.35 26 | cpos 1 1 0 27 | #link 28 | name bottom 29 | mass 3 30 | size 6 0.8 31 | prnt top 32 | cpos 2.8 0 0 33 | ppos -2.8 0 0 34 | physics 35 | angl 0.65 36 | cpos 1 -1 0 37 | #spline 38 | prnt bottom 39 | type bspline 40 | ppos -3.0 0 0 41 | 0 0 0 42 | 0 0 0 43 | 0 0 0 44 | 0 0 0 45 | 0 0 0 46 | 0 0 0 47 | 0 0 0 48 | 0 0 0 49 | 0 0 0 -------------------------------------------------------------------------------- /matlab/+redmax/BodyCuboid.m: -------------------------------------------------------------------------------- 1 | classdef BodyCuboid < redmax.Body 2 | %% 3 | properties 4 | sides % Side lengths 5 | end 6 | 7 | %% 8 | methods 9 | %% 10 | function this = BodyCuboid(density,sides) 11 | this = this@redmax.Body(density); 12 | this.sides = sides; 13 | end 14 | 15 | %% 16 | function computeInertia_(this) 17 | % Computes inertia at body 18 | this.I_i = se3.inertiaCuboid(this.sides,this.density); 19 | end 20 | 21 | %% 22 | function draw_(this) 23 | [F,V] = se3.patchCuboid(this.E_wi,this.sides); 24 | patch('Faces',F,'Vertices',V,'FaceColor',this.color); 25 | end 26 | 27 | %% 28 | function s = getAxisSize(this) 29 | s = min(this.sides); 30 | end 31 | end 32 | end 33 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/BodyCuboid.m: -------------------------------------------------------------------------------- 1 | classdef BodyCuboid < redmax.Body 2 | %% 3 | properties 4 | sides % Side lengths 5 | end 6 | 7 | %% 8 | methods 9 | %% 10 | function this = BodyCuboid(density,sides) 11 | this = this@redmax.Body(density); 12 | this.sides = sides; 13 | end 14 | 15 | %% 16 | function computeInertia_(this) 17 | % Computes inertia at body 18 | this.I_i = se3.inertiaCuboid(this.sides,this.density); 19 | end 20 | 21 | %% 22 | function draw_(this) 23 | [F,V] = se3.patchCuboid(this.E_wi,this.sides); 24 | patch('Faces',F,'Vertices',V,'FaceColor',this.color); 25 | end 26 | 27 | %% 28 | function s = getAxisSize(this) 29 | s = min(this.sides); 30 | end 31 | end 32 | end 33 | -------------------------------------------------------------------------------- /c++/PCG/resources/input2.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name root 3 | mass 1 4 | size 8 8 5 | cpos 1.0 6.4 0.0 6 | invisible 7 | #link 8 | name motor 9 | mass 0.25 10 | size 1 0.5 11 | prnt root 12 | cpos -0.25 0.0 0.0 13 | ppos 0 5 0 14 | angl 1 15 | type powered -10 16 | #link 17 | name top 18 | mass 3 19 | size 6 0.8 20 | prnt motor 21 | cpos 2.8 0 0 22 | ppos 0.75 0 0 23 | angl -0.5 24 | physics 25 | angl 0.35 26 | cpos 1 1 0 27 | #link 28 | name bottom 29 | mass 3 30 | size 6 0.8 31 | prnt top 32 | cpos 2.8 0 0 33 | ppos -2.8 0 0 34 | physics 35 | angl 0.65 36 | cpos 1 -1 0 37 | #spline 38 | prnt bottom 39 | type bspline 40 | ppos -3.0 0 0 41 | 0 0 0 42 | -1 0 0 43 | -2 0.25 0 44 | -1 0.875 0 45 | 0 1.5 0 46 | 1.5 0.85 0 47 | 3.0 0.25 0 48 | 2 0 0 49 | 1 0 0 -------------------------------------------------------------------------------- /matlab-simple/+redmax/BodyCuboid.m: -------------------------------------------------------------------------------- 1 | classdef BodyCuboid < redmax.Body 2 | %% 3 | properties 4 | sides % Side lengths 5 | end 6 | 7 | %% 8 | methods 9 | %% 10 | function this = BodyCuboid(density,sides) 11 | this = this@redmax.Body(density); 12 | this.sides = sides; 13 | end 14 | 15 | %% 16 | function computeInertia_(this) 17 | % Computes inertia at body 18 | this.I_i = se3.inertiaCuboid(this.sides,this.density); 19 | end 20 | 21 | %% 22 | function draw_(this) 23 | [F,V] = se3.patchCuboid(this.E_wi,this.sides); 24 | patch('Faces',F,'Vertices',V,'FaceColor',this.color); 25 | end 26 | 27 | %% 28 | function s = getAxisSize(this) 29 | s = min(this.sides); 30 | end 31 | end 32 | end 33 | -------------------------------------------------------------------------------- /c++/PCG/resources/input_test.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name root 3 | dens 1 4 | size 8 8 0.5 5 | cpos 1.0 6.4 0.0 6 | invisible 7 | #link 8 | name motor 9 | dens 0.25 10 | size 1 0.5 0.5 11 | prnt root 12 | cpos -0.25 0.0 0.0 13 | ppos 0 5 0 14 | angl 1 15 | type powered -10 16 | #link 17 | name top 18 | dens 3 19 | size 6 0.8 0.5 20 | prnt motor 21 | cpos 2.8 0 0 22 | ppos 0.75 0 0 23 | angl -0.5 24 | #link 25 | name bottom 26 | dens 3 27 | size 6 0.8 0.5 28 | prnt top 29 | cpos 2.8 0 0 30 | ppos -2.8 0 0 31 | #string 32 | lnka top 33 | lnkb bottom 34 | posa 0 0 0 35 | posb 0 0 0 36 | rest 5 37 | stif 0.05 38 | #spline 39 | prnt bottom 40 | type bspline 41 | ppos -3.0 0 0 42 | -6 2.2 0 43 | -4 0 0 44 | 0 -0.8 0 45 | 5.5 0 0 46 | 9.5 3 0 47 | 5 2 0 48 | 0 0.8 0 49 | -5 2 0 -------------------------------------------------------------------------------- /c++/PCG/resources/input_OPT.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name root 3 | mass 1 4 | size 8 8 5 | cpos 0.0 6.4 0.0 6 | invisible 7 | #link 8 | name motor 9 | mass 0.25 10 | size 1 0.5 11 | prnt root 12 | cpos -0.25 0.0 0.0 13 | ppos 1 5 0 14 | angl 1 15 | type powered -10 16 | #link 17 | name top 18 | mass 3 19 | size 6 0.8 20 | prnt motor 21 | cpos 2.8 0 0 22 | ppos 0.75 0 0 23 | angl -0.5 24 | physics 25 | angl 0.153417 26 | cpos 4.18719 -0.197702 0 27 | #link 28 | name bottom 29 | mass 3 30 | size 6 0.8 31 | prnt top 32 | cpos 2.8 0 0 33 | ppos -2.8 0 0 34 | physics 35 | angl 0.501175 36 | cpos -2.35627 -3.09295 0 37 | #spline 38 | prnt bottom 39 | type bspline 40 | ppos -3.0 0 0 41 | 0 0 0 42 | -1 0 0 43 | -2 0.25 0 44 | -1 0.875 0 45 | 0 1.5 0 46 | 1.5 0.85 0 47 | 3.0 0.25 0 48 | 2 0 0 49 | 1 0 0 -------------------------------------------------------------------------------- /c++/PCG/resources/input_OPT_123.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name root 3 | mass 1 4 | size 8 8 5 | cpos 0.0 6.4 0.0 6 | invisible 7 | #link 8 | name motor 9 | mass 0.25 10 | size 1 0.5 11 | prnt root 12 | cpos -0.25 0.0 0.0 13 | ppos 1 5 0 14 | angl 1 15 | type powered -10 16 | #link 17 | name top 18 | mass 3 19 | size 6 0.8 20 | prnt motor 21 | cpos 2.8 0 0 22 | ppos 0.75 0 0 23 | angl -0.5 24 | physics 25 | angl 0.852239 26 | cpos 2.09695 0.585139 0 27 | #link 28 | name bottom 29 | mass 3 30 | size 6 0.8 31 | prnt top 32 | cpos 2.8 0 0 33 | ppos -2.8 0 0 34 | physics 35 | angl 1.1695 36 | cpos 4.54036 -1.83031 0 37 | #spline 38 | prnt bottom 39 | type bspline 40 | ppos -3.0 0 0 41 | 0 0 0 42 | -1 0 0 43 | -2 0.25 0 44 | -1 0.875 0 45 | 0 1.5 0 46 | 1.5 0.85 0 47 | 3.0 0.25 0 48 | 2 0 0 49 | 1 0 0 -------------------------------------------------------------------------------- /c++/PCG/resources/input_bad.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name root 3 | mass 1 4 | size 8 8 5 | cpos 0.0 6.4 0.0 6 | invisible 7 | #link 8 | name motor 9 | mass 0.25 10 | size 1 0.5 11 | prnt root 12 | cpos -0.25 0.0 0.0 13 | ppos 1 5 0 14 | angl 1 15 | type powered -10 16 | #link 17 | name top 18 | mass 3 19 | size 6 0.8 20 | prnt motor 21 | cpos 2.8 0 0 22 | ppos 0.75 0 0 23 | angl -0.5 24 | physics 25 | angl 0.85299 26 | cpos 2.09695 0.585139 0 27 | #link 28 | name bottom 29 | mass 3 30 | size 6 0.8 31 | prnt top 32 | cpos 2.8 0 0 33 | ppos -2.8 0 0 34 | physics 35 | angl 0.341288 36 | cpos -4.55519 -1.82946 0 37 | #spline 38 | prnt bottom 39 | type bspline 40 | ppos -3.0 0 0 41 | 0 0 0 42 | -1 0 0 43 | -2 0.25 0 44 | -1 0.875 0 45 | 0 1.5 0 46 | 1.5 0.85 0 47 | 3.0 0.25 0 48 | 2 0 0 49 | 1 0 0 -------------------------------------------------------------------------------- /c++/PCG/resources/input_OPT_122.txt: -------------------------------------------------------------------------------- 1 | #link 2 | name root 3 | mass 1 4 | size 8 8 5 | cpos 0.0 6.4 0.0 6 | invisible 7 | #link 8 | name motor 9 | mass 0.25 10 | size 1 0.5 11 | prnt root 12 | cpos -0.25 0.0 0.0 13 | ppos 1 5 0 14 | angl 1 15 | type powered -10 16 | #link 17 | name top 18 | mass 3 19 | size 6 0.8 20 | prnt motor 21 | cpos 2.8 0 0 22 | ppos 0.75 0 0 23 | angl -0.5 24 | physics 25 | angl 0.153417 26 | cpos 4.18719 -0.197702 0 27 | #link 28 | name bottom 29 | mass 3 30 | size 6 0.8 31 | prnt top 32 | cpos 2.8 0 0 33 | ppos -2.8 0 0 34 | physics 35 | angl 0.501175 36 | cpos -2.35627 -3.09295 0 37 | #spline 38 | prnt bottom 39 | type bspline 40 | ppos -3.0 0 0 41 | 0 0 0 42 | -1 0 0 43 | -2 0.25 0 44 | -1 0.875 0 45 | 0 1.5 0 46 | 1.5 0.85 0 47 | 3.0 0.25 0 48 | 2 0 0 49 | 1 0 0 -------------------------------------------------------------------------------- /matlab-diff/+redmax/JointTranslational.m: -------------------------------------------------------------------------------- 1 | classdef JointTranslational < redmax.Joint 2 | % 3D translational joint 3 | 4 | %% 5 | properties 6 | end 7 | 8 | %% 9 | methods 10 | %% 11 | function this = JointTranslational(parent,body) 12 | this = this@redmax.Joint(parent,body,3); 13 | end 14 | end 15 | 16 | %% 17 | methods (Access = protected) 18 | 19 | %% 20 | function update_(this,deriv) 21 | this.Q(1:3,4) = this.q; 22 | this.A = se3.Ad(this.Q); 23 | this.S = [zeros(3);eye(3)]; 24 | this.Adot(4:6,1:3) = se3.brac(this.qdot); 25 | 26 | if deriv 27 | for k = 1 : 3 28 | ek = zeros(3,1); 29 | ek(k) = 1; 30 | ekbrac = se3.brac(ek); 31 | this.dAdq(4:6,1:3,k) = ekbrac; 32 | end 33 | end 34 | end 35 | end 36 | end 37 | -------------------------------------------------------------------------------- /matlab/cuboid.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.79 (sub 0) OBJ File: '' 2 | # www.blender.org 3 | v -0.500000 -1.000000 -1.500000 4 | v -0.500000 1.000000 -1.500000 5 | v 0.500000 1.000000 -1.500000 6 | v 0.500000 -1.000000 -1.500000 7 | v -0.500000 -1.000000 1.500000 8 | v -0.500000 1.000001 1.500000 9 | v 0.500000 1.000000 1.500000 10 | v 0.500000 -1.000000 1.500000 11 | vn -0.0000 0.0000 -1.0000 12 | vn 0.0000 0.0000 1.0000 13 | vn -1.0000 0.0000 -0.0000 14 | vn 0.0000 1.0000 -0.0000 15 | vn 1.0000 -0.0000 -0.0000 16 | vn -0.0000 -1.0000 0.0000 17 | s off 18 | f 2//1 4//1 1//1 19 | f 8//2 6//2 5//2 20 | f 5//3 2//3 1//3 21 | f 6//4 3//4 2//4 22 | f 3//5 8//5 4//5 23 | f 1//6 8//6 5//6 24 | f 2//1 3//1 4//1 25 | f 8//2 7//2 6//2 26 | f 5//3 6//3 2//3 27 | f 6//4 7//4 3//4 28 | f 3//5 7//5 8//5 29 | f 1//6 4//6 8//6 30 | -------------------------------------------------------------------------------- /c++/PCG/src/JointFixed.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Joint.h" 4 | 5 | struct LinkageSystem; 6 | struct State; 7 | struct StateDeriv; 8 | struct StateSolve; 9 | 10 | struct JointFixed : public Joint 11 | { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | JointFixed(std::unique_ptr &S, 15 | std::string pn, 16 | std::string cn, 17 | Eigen::Vector3d pp, 18 | Eigen::Vector3d cp, 19 | bool r, 20 | int i, 21 | Eigen::Matrix3d Q0); 22 | 23 | void updateMaxSparse(std::unique_ptr &SS, const std::unique_ptr &LS, const std::unique_ptr &S, Matrix6d &parentAdj, Matrix6d &childAdj, Eigen::Vector3d &g) const; 24 | 25 | private: 26 | void mapSelf(int c_i, std::unique_ptr &S); 27 | void updateSelf(); 28 | Vector6d Sqdot(const std::unique_ptr &S); 29 | }; -------------------------------------------------------------------------------- /c++/PCG/resources/cube.obj: -------------------------------------------------------------------------------- 1 | #### 2 | # 3 | # OBJ File Generated by Meshlab 4 | # 5 | #### 6 | # Object foot.obj 7 | # 8 | # Vertices: 8 9 | # Faces: 12 10 | # 11 | #### 12 | v 1.0 1.0 -1.0 13 | v 1.0 -1.0 -1.0 14 | v -1.0 -1.0 -1.0 15 | v -1.0 1.0 -1.0 16 | v 1.0 1.0 1.0 17 | v 1.0 -1.0 1.0 18 | v -1.0 -1.0 1.0 19 | v -1.0 1.0 1.0 20 | vn 0.000000 0.000000 -1.000000 21 | vn 0.000000 0.000000 1.000000 22 | vn 1.000000 -0.000000 -0.000000 23 | vn -0.000000 -1.000000 -0.000000 24 | vn -1.000000 0.000000 -0.000000 25 | vn 0.000000 1.000000 0.000000 26 | s off 27 | f 2//1 3//1 4//1 28 | f 8//2 7//2 6//2 29 | f 1//3 5//3 6//3 30 | f 2//4 6//4 7//4 31 | f 7//5 8//5 4//5 32 | f 1//6 4//6 8//6 33 | f 1//1 2//1 4//1 34 | f 5//2 8//2 6//2 35 | f 2//3 1//3 6//3 36 | f 3//4 2//4 7//4 37 | f 3//5 7//5 4//5 38 | f 5//6 1//6 8//6 39 | # 12 faces, 0 coords texture 40 | 41 | # End of File 42 | -------------------------------------------------------------------------------- /c++/PCG/src/StateDeriv.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define EIGEN_DONT_ALIGN_STATICALLY 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | // ----------------------------------------------------// 10 | // Stores all global state information needed for // 11 | // computation of the gradient // 12 | // ----------------------------------------------------// 13 | struct StateDeriv 14 | { 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | 17 | // stores pos & vel state history 18 | Eigen::VectorXd qBuf; 19 | // built backwards after simulation completes 20 | Eigen::VectorXd qhatBuf; 21 | 22 | // track the partials of force and obj_func across time 23 | std::vector c_dfdx; 24 | std::vector c_dfdv; 25 | std::vector c_dodx; 26 | std::vector c_dodv; 27 | }; -------------------------------------------------------------------------------- /c++/PCG/src/JointSlider.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Joint.h" 4 | 5 | struct LinkageSystem; 6 | struct State; 7 | struct StateDeriv; 8 | struct StateSolve; 9 | 10 | struct JointSlider : public Joint 11 | { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | JointSlider(std::unique_ptr &S, 15 | std::string pn, 16 | std::string cn, 17 | Eigen::Vector3d pp, 18 | Eigen::Vector3d cp, 19 | bool r, 20 | int i) : 21 | Joint(S, JType::Slider, pn, cn, pp, cp, r, i, 2) 22 | {}; 23 | 24 | void updateMaxSparse(std::unique_ptr &SS, const std::unique_ptr &LS, const std::unique_ptr &S, Matrix6d &parentAdj, Matrix6d &childAdj, Eigen::Vector3d &g) const; 25 | 26 | private: 27 | void mapSelf(int c_i, std::unique_ptr &S); 28 | void updateSelf(); 29 | Vector6d Sqdot(const std::unique_ptr &S) 30 | { 31 | return Vector6d::Zero(); 32 | }; 33 | 34 | }; -------------------------------------------------------------------------------- /c++/PCG/src/online/Brender/cpp/BrenderManager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @author: Gustavo Lopez 10-21-17 3 | * 4 | * @version: 1.0 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | class Brenderable; 15 | 16 | class BrenderManager 17 | { 18 | private: 19 | static bool instanceFlag_; 20 | static BrenderManager *manager_; 21 | int frame_; 22 | std::string EXPORT_DIR_; 23 | std::vector > brenderables_; 24 | BrenderManager() 25 | { 26 | //private constructor 27 | EXPORT_DIR_ = "."; 28 | frame_ = 0; 29 | } 30 | public: 31 | static BrenderManager* getInstance(); 32 | void setExportDir(std::string export_dir); 33 | int getFrame() const; 34 | void exportBrender(double time = 0.0); 35 | void add(std::shared_ptr brenderable); 36 | ~BrenderManager() 37 | { 38 | instanceFlag_ = false; 39 | } 40 | }; -------------------------------------------------------------------------------- /c++/PCG/src/JointHinge.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Joint.h" 4 | 5 | struct LinkageSystem; 6 | struct State; 7 | struct StateDeriv; 8 | struct StateSolve; 9 | 10 | struct JointHinge : public Joint 11 | { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | // axis of rotation 15 | Eigen::Vector3d axis; 16 | 17 | JointHinge(std::unique_ptr &S, 18 | std::string pn, 19 | std::string cn, 20 | Eigen::Vector3d pp, 21 | Eigen::Vector3d cp, 22 | bool r, 23 | int i, 24 | double angle, 25 | Eigen::Vector3d &axis, 26 | double k = 0, 27 | double d = 0); 28 | 29 | void updateMaxSparse(std::unique_ptr &SS, const std::unique_ptr &LS, const std::unique_ptr &S, Matrix6d &parentAdj, Matrix6d &childAdj, Eigen::Vector3d &g) const; 30 | 31 | private: 32 | void mapSelf(int c_i, std::unique_ptr &S); 33 | void updateSelf(); 34 | Vector6d Sqdot(const std::unique_ptr &S); 35 | }; -------------------------------------------------------------------------------- /c++/PCG/src/JointBall.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Joint.h" 4 | 5 | struct Block; 6 | struct LinkageSystem; 7 | struct State; 8 | struct StateDeriv; 9 | struct StateSolve; 10 | 11 | struct JointBall : public Joint 12 | { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | JointBall(std::unique_ptr &S, 16 | std::string pn, 17 | std::string cn, 18 | Eigen::Vector3d pp, 19 | Eigen::Vector3d cp, 20 | bool r, 21 | int i) : 22 | Joint(S, JType::Ball, pn, cn, pp, cp, r, i, 3) 23 | {}; 24 | 25 | void updateMaxSparse(std::unique_ptr &SS, const std::unique_ptr &LS, const std::unique_ptr &S, Matrix6d &parentAdj, Matrix6d &childAdj, Eigen::Vector3d &g) const; 26 | 27 | private: 28 | void mapSelf(int c_i, std::unique_ptr &S); 29 | void updateSelf(); 30 | Vector6d Sqdot(const std::unique_ptr &S) 31 | { 32 | Vector6d empty = Vector6d::Zero(); 33 | return empty; 34 | }; 35 | 36 | }; -------------------------------------------------------------------------------- /c++/PCG/src/JointPrismatic.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Joint.h" 4 | 5 | struct LinkageSystem; 6 | struct State; 7 | struct StateDeriv; 8 | struct StateSolve; 9 | 10 | struct JointPrismatic : public Joint 11 | { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | // axis of rotation 15 | Eigen::Vector3d axis; 16 | 17 | JointPrismatic(std::unique_ptr &S, 18 | std::string pn, 19 | std::string cn, 20 | Eigen::Vector3d pp, 21 | Eigen::Vector3d cp, 22 | bool r, 23 | int i, 24 | double angle, 25 | Eigen::Vector3d &axis, 26 | double k = 0, 27 | double d = 0); 28 | 29 | void updateMaxSparse(std::unique_ptr &SS, const std::unique_ptr &LS, const std::unique_ptr &S, Matrix6d &parentAdj, Matrix6d &childAdj, Eigen::Vector3d &g) const; 30 | 31 | private: 32 | void mapSelf(int c_i, std::unique_ptr &S); 33 | void updateSelf(); 34 | Vector6d Sqdot(const std::unique_ptr &S); 35 | }; -------------------------------------------------------------------------------- /matlab-diff/+redmax/JointPlanar.m: -------------------------------------------------------------------------------- 1 | classdef JointPlanar < redmax.Joint 2 | 3 | %% 4 | properties 5 | plane 6 | end 7 | 8 | %% 9 | methods 10 | %% 11 | function this = JointPlanar(parent,body,plane) 12 | this = this@redmax.Joint(parent,body,2); 13 | if nargin == 2 14 | plane = [1 0 0; 0 1 0]'; 15 | end 16 | this.plane(:,1) = plane(:,1)/norm(plane(:,1)); 17 | this.plane(:,2) = plane(:,2)/norm(plane(:,2)); 18 | end 19 | end 20 | 21 | %% 22 | methods (Access = protected) 23 | %% 24 | function update_(this,deriv) 25 | n = 2; 26 | B = this.plane; 27 | this.Q(1:3,4) = B*this.q; 28 | this.A = se3.Ad(this.Q); 29 | this.S = [zeros(3,2);B]; 30 | this.Adot(4:6,1:3) = se3.brac(B*this.qdot); 31 | 32 | if deriv 33 | this.dAdq = zeros(6,6,n); 34 | for k = 1 : 2 35 | bkbrac = se3.brac(B(:,k)); 36 | this.dAdq(4:6,1:3,k) = bkbrac; 37 | end 38 | end 39 | end 40 | end 41 | end 42 | -------------------------------------------------------------------------------- /matlab/+redmax/JointPlanar.m: -------------------------------------------------------------------------------- 1 | classdef JointPlanar < redmax.Joint 2 | % https://github.com/junggon/gear/blob/master/src/gjoint_planar.cpp 3 | % Assumes X-Y plane 4 | 5 | %% 6 | properties 7 | end 8 | 9 | %% 10 | methods 11 | %% 12 | function this = JointPlanar(parent,body) 13 | this = this@redmax.Joint(parent,body,2); 14 | end 15 | end 16 | 17 | %% 18 | methods (Access = protected) 19 | %% 20 | function update_(this) 21 | this.Q = eye(4); 22 | this.Q(1:2,4) = this.q(1:2); 23 | this.S(4,1) = 1.0; 24 | this.S(5,2) = 1.0; 25 | end 26 | 27 | %% 28 | function draw_(this) 29 | s = 2*min(this.body.sides); 30 | p = this.E_wj(1:3,4); 31 | x = this.E_wj(1:3,1); 32 | y = this.E_wj(1:3,2); 33 | px0 = p - s*x; 34 | px1 = p + s*x; 35 | py0 = p - s*y; 36 | py1 = p + s*y; 37 | ps = [px0 px1 nan(3,1) py0 py1]; 38 | plot3(ps(1,:),ps(2,:),ps(3,:),'k','LineWidth',2); 39 | end 40 | end 41 | end 42 | -------------------------------------------------------------------------------- /c++/PCG/src/JointUniversal.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Joint.h" 4 | 5 | struct LinkageSystem; 6 | struct State; 7 | struct StateDeriv; 8 | struct StateSolve; 9 | 10 | struct JointUniversal : public Joint 11 | { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | // axis of rotation 15 | Eigen::Vector3d axis; 16 | 17 | JointUniversal(std::unique_ptr &S, 18 | std::string pn, 19 | std::string cn, 20 | Eigen::Vector3d pp, 21 | Eigen::Vector3d cp, 22 | bool r, 23 | int i, 24 | double angleX, 25 | double angleY, 26 | double k = 0, 27 | double d = 0); 28 | 29 | void updateMaxSparse(std::unique_ptr &SS, const std::unique_ptr &LS, const std::unique_ptr &S, Matrix6d &parentAdj, Matrix6d &childAdj, Eigen::Vector3d &g) const; 30 | 31 | private: 32 | void mapSelf(int c_i, std::unique_ptr &S); 33 | void updateSelf(); 34 | Vector6d Sqdot(const std::unique_ptr &S); 35 | 36 | Eigen::Matrix3d R; 37 | }; -------------------------------------------------------------------------------- /c++/PCG/src/online/Brender/cpp/Brenderable.h: -------------------------------------------------------------------------------- 1 | /* 2 | * @author: Gustavo Lopez 10-21-17 3 | * 4 | * @version: 1.0 5 | */ 6 | 7 | #pragma once 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "BrenderManager.h" 13 | 14 | 15 | class Brenderable 16 | { 17 | public: 18 | enum BranderableType { 19 | Truncate, Append, ResetAppend 20 | }; 21 | 22 | Brenderable() {}; 23 | virtual ~Brenderable() {}; 24 | virtual int getBrenderCount() const { return 1; }; 25 | virtual std::vector getBrenderNames() const { return std::vector(1, ""); }; 26 | virtual std::vector getBrenderExtensions() const { return std::vector(1, ""); }; 27 | virtual std::vector getBrenderTypes() const { return std::vector(1, Truncate); }; 28 | virtual void exportBrender(std::vector< std::shared_ptr< std::ofstream > > outfiles, int frame, double time) const = 0; 29 | private: 30 | 31 | }; -------------------------------------------------------------------------------- /c++/PCG/src/JointPowered.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Joint.h" 4 | 5 | struct LinkageSystem; 6 | struct State; 7 | struct StateDeriv; 8 | struct StateSolve; 9 | 10 | struct JointPowered : public Joint 11 | { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | // Power constant 15 | Eigen::Vector3d cpower; 16 | JointPowered(){} 17 | JointPowered(std::unique_ptr &S, 18 | std::string pn, 19 | std::string cn, 20 | Eigen::Vector3d pp, 21 | Eigen::Vector3d cp, 22 | bool r, 23 | int i, 24 | Eigen::Vector3d &cpower) : 25 | Joint(S, JType::Powered, pn, cn, pp, cp, r, i, 1) 26 | { 27 | this->cpower = cpower; 28 | }; 29 | void updateMaxSparse(std::unique_ptr &SS, const std::unique_ptr &LS, const std::unique_ptr &S, Matrix6d &parentAdj, Matrix6d &childAdj, Eigen::Vector3d &g) const; 30 | 31 | private: 32 | void mapSelf(int c_i, std::unique_ptr &S); 33 | void updateSelf(); 34 | Vector6d Sqdot(const std::unique_ptr &S) 35 | { 36 | return Vector6d::Zero(); 37 | }; 38 | 39 | }; 40 | -------------------------------------------------------------------------------- /c++/PCG/src/online/Program.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef __Program__ 3 | #define __Program__ 4 | 5 | #include 6 | #include 7 | 8 | #define GLEW_STATIC 9 | #include 10 | 11 | /** 12 | * An OpenGL Program (vertex and fragment shaders) 13 | */ 14 | class Program 15 | { 16 | public: 17 | Program(); 18 | virtual ~Program(); 19 | 20 | void setVerbose(bool v) { verbose = v; } 21 | bool isVerbose() const { return verbose; } 22 | 23 | void setShaderNames(const std::string &v, const std::string &f); 24 | virtual bool init(); 25 | virtual void bind(); 26 | virtual void unbind(); 27 | 28 | void addAttribute(const std::string &name); 29 | void addUniform(const std::string &name); 30 | GLint getAttribute(const std::string &name) const; 31 | GLint getUniform(const std::string &name) const; 32 | 33 | protected: 34 | std::string vShaderName; 35 | std::string fShaderName; 36 | 37 | private: 38 | GLuint pid; 39 | std::map attributes; 40 | std::map uniforms; 41 | bool verbose; 42 | }; 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /matlab/+redmax/JointTranslational.m: -------------------------------------------------------------------------------- 1 | classdef JointTranslational < redmax.Joint 2 | % https://github.com/junggon/gear/blob/master/src/gjoint_translational.cpp 3 | 4 | %% 5 | properties 6 | end 7 | 8 | %% 9 | methods 10 | %% 11 | function this = JointTranslational(parent,body) 12 | this = this@redmax.Joint(parent,body,3); 13 | end 14 | end 15 | 16 | %% 17 | methods (Access = protected) 18 | %% 19 | function update_(this) 20 | this.Q = eye(4); 21 | this.Q(1:3,4) = this.q(1:3); 22 | this.S(4,1) = 1.0; 23 | this.S(5,2) = 1.0; 24 | this.S(6,3) = 1.0; 25 | end 26 | 27 | %% 28 | function draw_(this) 29 | s = 2*min(this.body.sides); 30 | p = this.E_wj(1:3,4); 31 | x = this.E_wj(1:3,1); 32 | y = this.E_wj(1:3,2); 33 | z = this.E_wj(1:3,3); 34 | px0 = p - s*x; 35 | px1 = p + s*x; 36 | py0 = p - s*y; 37 | py1 = p + s*y; 38 | pz0 = p - s*z; 39 | pz1 = p + s*z; 40 | ps = [px0 px1 nan(3,1) py0 py1 nan(3,1) pz0 pz1]; 41 | plot3(ps(1,:),ps(2,:),ps(3,:),'k','LineWidth',2); 42 | end 43 | end 44 | end 45 | -------------------------------------------------------------------------------- /c++/PCG/src/JointSpringy.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Joint.h" 4 | 5 | struct LinkageSystem; 6 | struct State; 7 | struct StateDeriv; 8 | struct StateSolve; 9 | 10 | 11 | struct JointSpringy : public Joint 12 | { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | // For springy joints 16 | double angle; 17 | double k; 18 | 19 | JointSpringy(std::unique_ptr &S, 20 | std::string pn, 21 | std::string cn, 22 | Eigen::Vector3d pp, 23 | Eigen::Vector3d cp, 24 | bool r, 25 | int i, 26 | double angle, 27 | double k) : 28 | Joint(S, JType::Springy, pn, cn, pp, cp, r, i, 1) 29 | { 30 | this->angle = angle; 31 | this->k = k; 32 | }; 33 | 34 | void updateMaxSparse(std::unique_ptr &SS, const std::unique_ptr &LS, const std::unique_ptr &S, Matrix6d &parentAdj, Matrix6d &childAdj, Eigen::Vector3d &g) const; 35 | 36 | protected: 37 | void mapSelf(int c_i, std::unique_ptr &S); 38 | void updateSelf(); 39 | Vector6d Sqdot(const std::unique_ptr &S) 40 | { 41 | return Vector6d::Zero(); 42 | }; 43 | 44 | }; -------------------------------------------------------------------------------- /matlab/+redmax/BodySphere.m: -------------------------------------------------------------------------------- 1 | classdef BodySphere < redmax.Body 2 | %BodySphere A sphere 3 | % 4 | 5 | %% 6 | properties 7 | radius 8 | end 9 | 10 | %% 11 | methods 12 | %% 13 | function this = BodySphere(density,radius) 14 | this = this@redmax.Body(density); 15 | this.radius = radius; 16 | end 17 | 18 | %% 19 | function computeInertia_(this) 20 | % Computes inertia at body 21 | r = this.radius; 22 | v = 4/3*pi*r^3; 23 | m = this.density*v; 24 | I = 2/5*m*r^2; 25 | this.I_i = [I I I m m m]'; 26 | end 27 | 28 | %% 29 | function draw_(this) 30 | n = 8; 31 | n1 = n+1; 32 | n1n1 = n1*n1; 33 | [X,Y,Z] = sphere(n); 34 | r = this.radius; 35 | X = r*reshape(X,1,n1n1); 36 | Y = r*reshape(Y,1,n1n1); 37 | Z = r*reshape(Z,1,n1n1); 38 | XYZ = this.E_wi*[X;Y;Z;ones(1,n1n1)]; 39 | X = reshape(XYZ(1,:),n1,n1); 40 | Y = reshape(XYZ(2,:),n1,n1); 41 | Z = reshape(XYZ(3,:),n1,n1); 42 | surf(X,Y,Z,'FaceColor',this.color); 43 | end 44 | 45 | %% 46 | function s = getAxisSize(this) 47 | s = 2*this.radius; 48 | end 49 | end 50 | end 51 | -------------------------------------------------------------------------------- /c++/PCG/src/LinkageSystem.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | class Constraint; 9 | struct Block; 10 | struct Joint; 11 | 12 | // ----------------------------------------------------// 13 | // A collection of blocks(links, rigid bodies) that // 14 | // are connected by joints to make a linkage system // 15 | // ----------------------------------------------------// 16 | struct LinkageSystem 17 | { 18 | // the links, blocks, rigid_bodies 19 | std::vector< std::pair < std::string, std::shared_ptr > > blocks; 20 | // the joints between objects in the linkage system 21 | std::vector> joints; 22 | // the features that apply constraints to the rigid bodies 23 | std::vector> constraints; 24 | 25 | // reduced coordinate ordering 26 | std::vector joint_map; 27 | 28 | std::shared_ptr find(std::string name) 29 | { 30 | for (int i = 0; i < blocks.size(); ++i) 31 | { 32 | if (blocks[i].first.compare(name) == 0) 33 | return blocks[i].second; 34 | } 35 | 36 | return nullptr; 37 | } 38 | }; -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Shinjiro Sueda 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/JointPrismatic.m: -------------------------------------------------------------------------------- 1 | classdef JointPrismatic < redmax.Joint 2 | % https://github.com/junggon/gear/blob/master/src/gjoint_prismatic.cpp 3 | 4 | %% 5 | properties 6 | axis 7 | sides 8 | end 9 | 10 | %% 11 | methods 12 | %% 13 | function this = JointPrismatic(parent,body,axis) 14 | this = this@redmax.Joint(parent,body,1); 15 | this.axis = axis/norm(axis); 16 | this.sides = [0.5 0.5 0.5]; 17 | end 18 | 19 | %% 20 | function setGeometry(this,sides) 21 | this.sides = sides; 22 | end 23 | end 24 | 25 | %% 26 | methods (Access = protected) 27 | %% 28 | function update_(this,deriv) 29 | a = this.axis; 30 | p = a*this.q; 31 | qdot = this.qdot; 32 | 33 | this.Q(1:3,4) = p; 34 | this.A = se3.Ad(this.Q); 35 | this.S = [zeros(3,1);a]; 36 | abrac = se3.brac(a); 37 | this.Adot(4:6,1:3) = abrac*qdot; 38 | 39 | if deriv 40 | this.dAdq(4:6,1:3) = abrac; 41 | end 42 | end 43 | 44 | %% 45 | function draw_(this) 46 | E_wj = this.body.E_wi*this.body.E0_ij; 47 | [F,V] = se3.patchCuboid(E_wj,this.sides); 48 | patch('Faces',F,'Vertices',V,'FaceColor',this.body.color); 49 | end 50 | end 51 | end 52 | -------------------------------------------------------------------------------- /matlab/+redmax/JointFree2D.m: -------------------------------------------------------------------------------- 1 | classdef JointFree2D < redmax.Joint 2 | % 2D free joint 3 | % 4 | 5 | %% 6 | properties 7 | jointP % XY planar 8 | jointR % Z revolute 9 | end 10 | 11 | %% 12 | methods 13 | %% 14 | function this = JointFree2D(parent,body) 15 | this = this@redmax.Joint(parent,body,3); 16 | this.jointP = redmax.JointPlanar([],body); 17 | this.jointR = redmax.JointRevolute([],body,[0 0 1]'); 18 | body.joint = this; 19 | end 20 | end 21 | 22 | %% 23 | methods (Access = protected) 24 | %% 25 | function update_(this) 26 | this.jointP.q = this.q(1:2); 27 | this.jointR.q = this.q(3); 28 | this.jointP.qdot = this.qdot(1:2); 29 | this.jointR.qdot = this.qdot(3); 30 | this.jointP.update_(); 31 | this.jointR.update_(); 32 | 33 | Qp = this.jointP.Q; 34 | Qr = this.jointR.Q; 35 | this.Q = Qp*Qr; 36 | 37 | s = sin(this.q(3)); 38 | c = cos(this.q(3)); 39 | qdotr = this.qdot(3); 40 | 41 | this.S = [ 42 | 0 0 0 c -s 0 43 | 0 0 0 s c 0 44 | 0 0 1 0 0 0]'; 45 | this.Sdot = [ 46 | 0 0 0 -qdotr*s -qdotr*c 0 47 | 0 0 0 qdotr*c -qdotr*s 0 48 | 0 0 0 0 0 0]'; 49 | end 50 | end 51 | end 52 | -------------------------------------------------------------------------------- /c++/PCG/src/Rigid.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _RIGID_H_ 3 | #define _RIGID_H_ 4 | 5 | #define EIGEN_USE_MKL 6 | #define EIGEN_DONT_ALIGN_STATICALLY 7 | #include 8 | #include 9 | 10 | typedef Eigen::Matrix Vector6d; 11 | typedef Eigen::Matrix Matrix6d; 12 | typedef Eigen::Matrix Matrix3x6d; 13 | typedef Eigen::Matrix Matrix6x3d; 14 | 15 | class Rigid 16 | { 17 | private: 18 | Rigid(); 19 | 20 | public: 21 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 22 | 23 | static Eigen::Matrix4d inverse(const Eigen::Matrix4d &E); 24 | static Matrix3x6d gamma(const Eigen::Vector3d &r); 25 | static Matrix6d adjoint(const Eigen::Matrix4d &E); 26 | static Matrix6d addot(const Eigen::Matrix4d &E, const Vector6d &phi); 27 | static Eigen::Matrix3d bracket3(const Eigen::Vector3d &a); 28 | static Eigen::Matrix4d bracket6(const Vector6d &a); 29 | static Eigen::Vector3d unbracket3(const Eigen::Matrix3d &A); 30 | static Vector6d unbracket6(const Eigen::Matrix4d &A); 31 | static Eigen::Matrix4d integrate(const Eigen::Matrix4d &E0, const Eigen::VectorXd &phi, double h); 32 | static Vector6d log(const Eigen::Matrix4d &A); 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /c++/PCG/src/online/GLSL.h: -------------------------------------------------------------------------------- 1 | // 2 | // Many useful helper functions for GLSL shaders - gleaned from various sources including orange book 3 | // Created by zwood on 2/21/10. 4 | // Modified by sueda 10/15/15. 5 | // 6 | 7 | #pragma once 8 | #ifndef __GLSL__ 9 | #define __GLSL__ 10 | 11 | #define GLEW_STATIC 12 | #include 13 | 14 | /////////////////////////////////////////////////////////////////////////////// 15 | // For printing out the current file and line number // 16 | /////////////////////////////////////////////////////////////////////////////// 17 | #include 18 | 19 | template 20 | std::string NumberToString(T x) 21 | { 22 | std::ostringstream ss; 23 | ss << x; 24 | return ss.str(); 25 | } 26 | 27 | #define GET_FILE_LINE (std::string(__FILE__) + ":" + NumberToString(__LINE__)).c_str() 28 | /////////////////////////////////////////////////////////////////////////////// 29 | 30 | namespace GLSL { 31 | 32 | void checkVersion(); 33 | void checkError(const char *str = 0); 34 | void printProgramInfoLog(GLuint program); 35 | void printShaderInfoLog(GLuint shader); 36 | int textFileWrite(const char *filename, const char *s); 37 | char *textFileRead(const char *filename); 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /matlab/+redmax/ConstraintPrescJoint.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintPrescJoint < redmax.Constraint 2 | % ConstraintPrescJoint Prescribes joint motion. 3 | 4 | %% 5 | properties 6 | joint 7 | vel % velocity-level integrator? 8 | q 9 | qdot 10 | qddot 11 | end 12 | 13 | %% 14 | methods 15 | %% 16 | function this = ConstraintPrescJoint(joint,vel) 17 | this = this@redmax.Constraint(0,joint.ndof,0,0); 18 | this.name = [joint.name,'-presc']; 19 | joint.presc = this; 20 | this.joint = joint; 21 | this.vel = vel; 22 | this.q = zeros(joint.ndof,1); 23 | this.qdot = zeros(joint.ndof,1); 24 | this.qddot = zeros(joint.ndof,1); 25 | end 26 | end 27 | 28 | %% 29 | methods (Access = protected) 30 | %% 31 | function [Gr,Grdot,gr,grdot,grddot] = computeJacEqR_(this,Gr,Grdot,gr,grdot,grddot) 32 | rows = this.idxER; 33 | cols = this.joint.idxR; 34 | this.idxQ = cols; 35 | Gr(rows,cols) = -eye(this.joint.ndof); 36 | gr(rows) = this.q - this.joint.q; 37 | if this.vel 38 | grdot(rows) = this.qdot; 39 | else 40 | grdot(rows) = this.joint.qdot; 41 | grddot(rows) = this.qddot; 42 | end 43 | end 44 | 45 | %% 46 | function scatterForceEqR_(this,Grt,lr) %#ok 47 | this.joint.tau = this.fcon; 48 | end 49 | end 50 | end 51 | -------------------------------------------------------------------------------- /matlab/+redmax/ConstraintPrescBody.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintPrescBody < redmax.Constraint 2 | % ConstraintPrescBody Prescribes body motion. 3 | 4 | %% 5 | properties 6 | body 7 | prows % Prescribed motion rows 8 | vel % velocity-level integrator? 9 | q 10 | qdot 11 | qddot 12 | end 13 | 14 | %% 15 | methods 16 | %% 17 | function this = ConstraintPrescBody(body,prows,vel) 18 | this = this@redmax.Constraint(length(prows),0,0,0); 19 | this.name = [body.name,'-presc']; 20 | body.presc = this; 21 | this.body = body; 22 | this.prows = prows; 23 | this.vel = vel; 24 | this.q = zeros(6,1); 25 | this.qdot = zeros(6,1); 26 | this.qddot = zeros(6,1); 27 | end 28 | end 29 | 30 | %% 31 | methods (Access = protected) 32 | %% 33 | function [Gm,Gmdot,gm,gmdot,gmddot] = computeJacEqM_(this,Gm,Gmdot,gm,gmdot,gmddot) 34 | rows = this.idxEM; 35 | cols = this.body.idxM; 36 | this.idxQ = cols; 37 | I = eye(6); 38 | Gm(rows,cols) = -I(this.prows,:); 39 | if this.vel 40 | gmdot(rows) = this.qdot(this.prows); 41 | else 42 | gmdot(rows) = this.body.phi(this.prows); 43 | gmddot(rows) = this.qddot(this.prows); 44 | end 45 | end 46 | 47 | %% 48 | function scatterForceEqM_(this,Gmt,lm) %#ok 49 | this.body.wext_i = this.fcon; 50 | end 51 | end 52 | end 53 | -------------------------------------------------------------------------------- /matlab/+redmax/ConstraintMultQ.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintMultQ < redmax.Constraint 2 | % ConstraintMultQ Constrains qs to be a multiple of each other 3 | 4 | %% 5 | properties 6 | jointA % One joint 7 | jointB % Another joint 8 | factor % Multiplication factor 9 | end 10 | 11 | %% 12 | methods 13 | %% 14 | function this = ConstraintMultQ(jointA,jointB) 15 | this = this@redmax.Constraint(0,1,0,0); 16 | this.name = [jointA.name,'-Q-',jointB.name]; 17 | this.jointA = jointA; 18 | this.jointB = jointB; 19 | this.factor = 1.0; 20 | rev = 'redmax.JointRevolute'; 21 | if ~isa(jointA,rev) || ~isa(jointB,rev) 22 | error('ConstraintMultQ only works between revolute joints'); 23 | end 24 | end 25 | 26 | %% 27 | function setFactor(this,factor) 28 | this.factor = factor; 29 | end 30 | end 31 | 32 | %% 33 | methods (Access = protected) 34 | %% 35 | function [Gr,Grdot,gr,grdot,grddot] = computeJacEqR_(this,Gr,Grdot,gr,grdot,grddot) 36 | % Constraint: qB = factor*qA 37 | % factor*qdotA - qdotB = 0 38 | % Gr = [factor -1] 39 | % Grdot = 0 40 | rows = this.idxER; 41 | colsA = this.jointA.idxR; 42 | colsB = this.jointB.idxR; 43 | this.idxQ = [colsA colsB]; 44 | Gr(rows,colsA) = this.factor; 45 | Gr(rows,colsB) = -1; 46 | gr(rows) = this.factor*this.jointA.q(1) - this.jointB.q(1); 47 | end 48 | end 49 | end 50 | -------------------------------------------------------------------------------- /matlab/+redmax/JointUniversal.m: -------------------------------------------------------------------------------- 1 | classdef JointUniversal < redmax.Joint 2 | % https://github.com/junggon/gear/blob/master/src/gjoint_universal.cpp 3 | % Assumes XY axes 4 | 5 | %% 6 | properties 7 | end 8 | 9 | %% 10 | methods 11 | %% 12 | function this = JointUniversal(parent,body) 13 | this = this@redmax.Joint(parent,body,2); 14 | end 15 | end 16 | 17 | %% 18 | methods (Access = protected) 19 | %% 20 | function update_(this) 21 | q0 = this.q(1); q1 = this.q(2); 22 | dq0 = this.qdot(1); dq1 = this.qdot(2); %#ok 23 | c0 = cos(q0); c1 = cos(q1); 24 | s0 = sin(q0); s1 = sin(q1); 25 | this.Q = eye(4); 26 | this.Q(1:3,1) = [c1, s0*s1, -c0*s1]'; 27 | this.Q(1:3,2) = [0, c0, s0]'; 28 | this.Q(1:3,3) = [s1, -s0*c1, c0*c1]'; 29 | %S[0] = c1; S[2] = s1; S[7] = 1; 30 | this.S(1,1) = c1; 31 | this.S(3,1) = s1; 32 | this.S(2,2) = 1; 33 | %dS[0] = -s1*dq1; dS[2] = c1*dq1; 34 | this.Sdot(1,1) = -s1*dq1; 35 | this.Sdot(3,1) = c1*dq1; 36 | end 37 | 38 | %% 39 | function draw_(this) 40 | s = 2*min(this.body.sides); 41 | p = this.E_wj(1:3,4); 42 | x = this.E_wj(1:3,1); 43 | y = this.E_wj(1:3,2); 44 | px0 = p - s*x; 45 | px1 = p + s*x; 46 | py0 = p - s*y; 47 | py1 = p + s*y; 48 | ps = [px0 px1 nan(3,1) py0 py1]; 49 | plot3(ps(1,:),ps(2,:),ps(3,:),'k','LineWidth',2); 50 | end 51 | end 52 | end 53 | -------------------------------------------------------------------------------- /c++/PCG/src/Cuboid.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _CUBOID_H_ 3 | #define _CUBOID_H_ 4 | 5 | #define EIGEN_DONT_ALIGN_STATICALLY 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | class Program; 14 | 15 | typedef Eigen::Matrix Matrix6d; 16 | 17 | /** 18 | * A shape defined by a list of triangles 19 | * - posBuf should be of length 3*ntris 20 | * - norBuf should be of length 3*ntris (if normals are available) 21 | * - texBuf should be of length 2*ntris (if texture coords are available) 22 | * posBufID, norBufID, and texBufID are OpenGL buffer identifiers. 23 | */ 24 | class Cuboid 25 | { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | 29 | Cuboid(); 30 | virtual ~Cuboid(); 31 | void loadMesh(const std::string &meshName); 32 | void size(const Eigen::Vector3d &size); 33 | void init(); 34 | void updatePosBuf(const Eigen::Matrix4d& E); 35 | void draw(const std::unique_ptr &prog) const; 36 | 37 | // Matrix6d spatialInertia(double mass); 38 | std::vector cornerPos(const Eigen::Matrix4d& E); 39 | 40 | private: 41 | float Xdim; 42 | float Ydim; 43 | float Zdim; 44 | 45 | std::vector posBuf; 46 | std::vector posBufOrigin; 47 | std::vector norBuf; 48 | std::vector texBuf; 49 | unsigned posBufID; 50 | unsigned norBufID; 51 | unsigned texBufID; 52 | }; 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /c++/PCG/src/Scene.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef __Scene__ 3 | #define __Scene__ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #define EIGEN_DONT_ALIGN_STATICALLY 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #include "online/Brender/cpp/BrenderManager.h" 16 | 17 | class Camera; 18 | class Program; 19 | class MatrixStack; 20 | class Optimizer; 21 | 22 | class RigidBodyMain; 23 | 24 | class Scene 25 | { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | 29 | Scene(const std::string &RESOURCE_DIR); 30 | ~Scene(); 31 | 32 | void init(); 33 | void load(const std::string &FILENAME, std::shared_ptr &c); 34 | 35 | int keyPressed(unsigned int key); 36 | int clickPress(float x, float y, float z); 37 | int clickRelease(float x, float y, float z); 38 | 39 | Eigen::VectorXd step(); 40 | void batchTest(); 41 | void displayActions(); 42 | 43 | double getTime() const { return t; }; 44 | 45 | // hand-off (main -> rigidbodylinkage) functions 46 | void draw(std::unique_ptr &MV, const std::unique_ptr &prog) const; 47 | void drawJoints(std::unique_ptr &MV) const; 48 | void drawPoints(std::unique_ptr &MV, const std::unique_ptr &prog) const; 49 | 50 | private: 51 | double t; 52 | double h; 53 | 54 | std::shared_ptr rigid_body; 55 | BrenderManager *brender; 56 | }; 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /c++/PCG/src/ChronoTimer.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | class ChronoTimer 5 | { 6 | private: 7 | std::string name; 8 | int ntimers; 9 | std::vector time; 10 | std::vector count; 11 | std::vector< std::chrono::time_point > start; 12 | 13 | public: 14 | ChronoTimer(const std::string &name, int ntimers = 1) 15 | { 16 | this->name = name; 17 | this->ntimers = ntimers; 18 | reset(); 19 | } 20 | 21 | void reset() 22 | { 23 | time.resize(ntimers); 24 | count.resize(ntimers); 25 | start.resize(ntimers); 26 | } 27 | 28 | void tic(int i = 0) 29 | { 30 | start[i] = std::chrono::steady_clock::now(); 31 | } 32 | 33 | void toc(int i = 0) 34 | { 35 | auto end = std::chrono::steady_clock::now(); 36 | auto diff = end - start[i]; 37 | time[i] += std::chrono::duration(diff).count(); 38 | ++count[i]; 39 | } 40 | 41 | void print() const 42 | { 43 | if (ntimers == 1) { 44 | printf("%s % 8d %0.9f\n", name.c_str(), count[0], time[0] * 1e-9); 45 | } 46 | else { 47 | printf("%s[0] % 8d %0.9f\n", name.c_str(), count[0], time[0] * 1e-9); 48 | for (int i = 1; i < ntimers; ++i) { 49 | printf("%s[%d] % 8d %0.9f\n", name.c_str(), i, count[i], time[i] * 1e-9); 50 | } 51 | } 52 | } 53 | }; 54 | 55 | //int main(int argc, char **argv) 56 | //{ 57 | // ChronoTimer timer("Test"); 58 | // timer.tic(); 59 | // // some code 60 | // timer.toc(); 61 | // timer.print(); 62 | //} -------------------------------------------------------------------------------- /matlab/+redmax/JointFree3D.m: -------------------------------------------------------------------------------- 1 | classdef JointFree3D < redmax.Joint 2 | % 3D free joint 3 | % This composite joint uses JointTranslational as S1 and 4 | % JointSphericalExp as S2, unlike JointFree. This version behaves 5 | % better with Euler. 6 | % 7 | 8 | %% 9 | properties 10 | joint1 11 | joint2 12 | end 13 | 14 | %% 15 | methods 16 | %% 17 | function this = JointFree3D(parent,body) 18 | this = this@redmax.Joint(parent,body,6); 19 | this.joint1 = redmax.JointTranslational([],body); 20 | this.joint2 = redmax.JointSphericalExp([],body); 21 | body.joint = this; 22 | end 23 | end 24 | 25 | %% 26 | methods (Access = protected) 27 | %% 28 | function reparam_(this) 29 | this.joint2.reparam_(); 30 | this.q(4:6) = this.joint2.q; 31 | this.qdot(4:6) = this.joint2.qdot; 32 | end 33 | 34 | %% 35 | function update_(this) 36 | this.joint1.q = this.q(1:3); 37 | this.joint2.q = this.q(4:6); 38 | this.joint1.qdot = this.qdot(1:3); 39 | this.joint2.qdot = this.qdot(4:6); 40 | this.joint1.update_(); 41 | this.joint2.update_(); 42 | 43 | Q1 = this.joint1.Q; 44 | Q2 = this.joint2.Q; 45 | this.Q = Q1*Q2; 46 | 47 | S2 = this.joint2.S; 48 | S2hat = S2(1:3,1:3); 49 | R_21 = Q2(1:3,1:3)'; 50 | this.S(4:6,1:3) = R_21; 51 | this.S(1:3,4:6) = S2hat; 52 | 53 | dS2 = this.joint2.Sdot; 54 | this.Sdot(4:6,1:3) = -se3.brac(S2hat*this.qdot(4:6))*R_21; 55 | this.Sdot(1:3,4:6) = dS2(1:3,1:3); 56 | end 57 | end 58 | end 59 | -------------------------------------------------------------------------------- /c++/PCG/README.md: -------------------------------------------------------------------------------- 1 | # REDMAX: Efficient & Flexible Approach for Articulated Dynamics 2 | 3 | > C++ implementation including Projected Block Jacobi Preconditioner 4 | --- 5 | ## Installation 6 | 7 | The code was developed on Windows and has been tested on MacOS. 8 | 9 | ### Clone 10 | 11 | - Clone this repo to your local machine using 12 | ```shell 13 | $ git clone https://github.com/sueda/redmax.git 14 | $ cd redmax/c++/PCG/ 15 | $ mkdir build 16 | ``` 17 | 18 | ### Dependencies 19 | 20 | Required: 21 | 22 | * [CMake](https://cmake.org/ "CMake") 23 | * [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page "Eigen") 24 | * [JsonCpp](https://github.com/open-source-parsers/jsoncpp "JsonCpp") 25 | * [Intel® MKL](https://software.intel.com/en-us/mkl "MKL") 26 | 27 | To run the simulation "online" with rendered output: 28 | 29 | * [GLM](https://glm.g-truc.net/0.9.9/index.html "GLM") 30 | * [GLFW](http://www.glfw.org/ "GLFW") 31 | * [GLEW](http://glew.sourceforge.net/ "GLEW") 32 | 33 | Optional: 34 | 35 | PARDISO is strongly recommended if you want to reproduce some results in the paper. 36 | 37 | * [PARDISO](https://www.pardiso-project.org/ "PARDISO") 38 | 39 | ### Setup 40 | 41 | - Compile the code using CMake: 42 | 43 | ```shell 44 | $ cd build 45 | $ cmake [options] .. 46 | $ make 47 | ``` 48 | --- 49 | ## Usage 50 | 51 | ### Features 52 | ### Command Line Switches 53 | 54 | --- 55 | 56 | ## Contact 57 | 58 | If you would like to contact us for anything regarding REDMAX feel free to email us. 59 | 60 | --- 61 | -------------------------------------------------------------------------------- /c++/PCG/cmake/FindGLM.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Find GLM 3 | # 4 | # Try to find GLM : OpenGL Mathematics. 5 | # This module defines 6 | # - GLM_INCLUDE_DIRS 7 | # - GLM_FOUND 8 | # 9 | # The following variables can be set as arguments for the module. 10 | # - GLM_ROOT_DIR : Root library directory of GLM 11 | # 12 | # References: 13 | # - https://github.com/Groovounet/glm/blob/master/util/FindGLM.cmake 14 | # - https://bitbucket.org/alfonse/gltut/src/28636298c1c0/glm-0.9.0.7/FindGLM.cmake 15 | # - https://github.com/lighttransport/nanogi/blob/master/cmake/FindGLM.cmake 16 | 17 | # Additional modules 18 | include(FindPackageHandleStandardArgs) 19 | 20 | if (WIN32) 21 | # Find include files 22 | find_path( 23 | GLM_INCLUDE_DIR 24 | NAMES glm/glm.hpp 25 | PATHS 26 | $ENV{PROGRAMFILES}/include/ 27 | ${GLM_ROOT_DIR}/include/ 28 | ${GLM_ROOT_DIR}/ 29 | DOC "The directory where glm/glm.hpp resides") 30 | else() 31 | # Find include files 32 | find_path( 33 | GLM_INCLUDE_DIR 34 | NAMES glm/glm.hpp 35 | PATHS 36 | /usr/include/ 37 | /usr/local/include/ 38 | /sw/include/ 39 | /opt/local/include/ 40 | $ENV{GLM_ROOT_DIR}/ 41 | ${GLM_ROOT_DIR}/include/ 42 | ${GLM_ROOT_DIR}/ 43 | DOC "The directory where glm/glm.hpp resides") 44 | endif() 45 | 46 | # Handle REQUIRD argument, define *_FOUND variable 47 | find_package_handle_standard_args(GLM DEFAULT_MSG GLM_INCLUDE_DIR) 48 | 49 | # Define GLM_INCLUDE_DIRS 50 | if (GLM_FOUND) 51 | set(GLM_INCLUDE_DIRS ${GLM_INCLUDE_DIR}) 52 | endif() 53 | 54 | # Hide some variables 55 | mark_as_advanced(GLM_INCLUDE_DIR) -------------------------------------------------------------------------------- /matlab/README.md: -------------------------------------------------------------------------------- 1 | We provide a reference implementation of the RedMax algorithm, written in object-oriented MATLAB (2018b). This code is not designed for performance but is rather designed for pedagogical purposes. To run the code, go to the directory containing testRedMax.m and type: 2 | 3 | ``` 4 | >> testRedMax(1,0) 5 | ``` 6 | This will show a swinging chain with alternating revolute/fixed joints. The first two arguments modify the integrator type and the scene ID: 7 | 8 | (1) `itype`: Integrator type 9 | - `1` Use the recursive O(n) algorithm [Kim and Pollard 2011; Kim 2012] with ode45. 10 | - `2` Use RedMax with ode45. This gives numerically the same solution as the recursive O(n) algorithm. 11 | - `3` Use RedMax with linearly implicit Euler. This is the most feature-rich option. 12 | (2) `sceneID`: There are many preset scenes. The first few are: 13 | - `0` Simple serial chain 14 | - `1` Different revolute axes 15 | - `2` Branching 16 | - `3` Spherical joint 17 | - `4` Loop 18 | - `5` ··· 19 | 20 | The full list of scenes is in testRedMax.m. The other arguments are used to control what gets displayed: 21 | 22 | (3) `drawScene`: Whether to draw the scene. Thiscansignificantly speed up the program, since drawing in MATLAB is very slow. 23 | (4) `plotH`: Whether to compute and plot the energy over time. 24 | 25 | To run all the test cases, copy and paste the following into the command window: 26 | 27 | ``` 28 | clear; clc; 29 | for itype = 1 : 3 30 | for sceneID = 0 : 35 31 | testRedMax(itype,sceneID,false,false); 32 | end 33 | end 34 | ``` 35 | -------------------------------------------------------------------------------- /c++/PCG/src/online/Camera.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef __Camera__ 3 | #define __Camera__ 4 | 5 | #include 6 | 7 | #define GLM_FORCE_RADIANS 8 | #include 9 | 10 | class MatrixStack; 11 | 12 | class Camera 13 | { 14 | public: 15 | enum { 16 | ROTATE = 0, 17 | TRANSLATE, 18 | SCALE 19 | }; 20 | 21 | Camera(); 22 | virtual ~Camera(); 23 | void setInitDistance(const float z) { translations.z = -std::abs(z); } 24 | void setAspect(const float a) { aspect = a; }; 25 | void setFovy(const float f) { fovy = f; }; 26 | void setZnear(const float z) { znear = z; }; 27 | void setZfar(const float z) { zfar = z; }; 28 | void setRotationFactor(const float f) { rfactor = f; }; 29 | void setTranslationFactor(const float f) { tfactor = f; }; 30 | void setScaleFactor(const float f) { sfactor = f; }; 31 | void mouseClicked(const float x, const float y, const bool shift, const bool ctrl, const bool alt); 32 | void mouseMoved(const float x, const float y); 33 | void applyProjectionMatrix(const std::unique_ptr &P) const; 34 | void applyViewMatrix(const std::unique_ptr &MV) const; 35 | 36 | void setTranslations(const float x, const float y, const float z); 37 | 38 | float getZnear() { return znear; }; 39 | float getZfar() { return zfar; }; 40 | 41 | private: 42 | float aspect; 43 | float fovy; 44 | float znear; 45 | float zfar; 46 | glm::vec2 rotations; 47 | glm::vec3 translations; 48 | glm::vec2 mousePrev; 49 | int state; 50 | float rfactor; 51 | float tfactor; 52 | float sfactor; 53 | float winz; 54 | }; 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /c++/PCG/src/JointBall.cpp: -------------------------------------------------------------------------------- 1 | #include "JointBall.h" 2 | 3 | #include "Block.h" 4 | #include "LinkageSystem.h" 5 | #include "State.h" 6 | #include "StateDeriv.h" 7 | #include "StateSolve.h" 8 | 9 | void JointBall::updateMaxSparse(std::unique_ptr& SS, const std::unique_ptr& LS, const std::unique_ptr& S, Matrix6d & parentAdj, Matrix6d & childAdj, Eigen::Vector3d & g) const 10 | { 11 | int numObj = (int)LS->blocks.size(); 12 | int row = SS->row; 13 | 14 | int parentIndex; 15 | int childIndex; 16 | if (!root) 17 | { 18 | parentIndex = LS->find(parentName)->joint->jindex; 19 | } 20 | childIndex = LS->find(childName)->joint->jindex; 21 | 22 | // Constrain translational movement 23 | for (int r = 0; r < 3; ++r) 24 | { 25 | for (int c = 0; c < 6; ++c) 26 | { 27 | if (!root) 28 | { 29 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, parentIndex * 6 + c, parentAdj(r + 3, c))); 30 | SS->LHSlist.push_back(Eigen::Triplet(parentIndex * 6 + c, numObj * 6 + row + r, parentAdj(r + 3, c))); 31 | } 32 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, childIndex * 6 + c, -1 * childAdj(r + 3, c))); 33 | SS->LHSlist.push_back(Eigen::Triplet(childIndex * 6 + c, numObj * 6 + row + r, -1 * childAdj(r + 3, c))); 34 | } 35 | // Load the corrective factor -1/h*g into f 36 | SS->fn[numObj * 6 + row + r] = g[r]; 37 | } 38 | row = row + 3; 39 | 40 | SS->row = row; 41 | } 42 | 43 | void JointBall::mapSelf(int c_i, std::unique_ptr& S) 44 | { 45 | } 46 | 47 | void JointBall::updateSelf() 48 | { 49 | } 50 | -------------------------------------------------------------------------------- /c++/PCG/src/JointSlider.cpp: -------------------------------------------------------------------------------- 1 | #include "JointSlider.h" 2 | 3 | #include "RigidBodyUtility.h" 4 | 5 | #include "Block.h" 6 | #include "LinkageSystem.h" 7 | #include "State.h" 8 | #include "StateDeriv.h" 9 | #include "StateSolve.h" 10 | 11 | void JointSlider::updateMaxSparse(std::unique_ptr& SS, const std::unique_ptr& LS, const std::unique_ptr& S, Matrix6d & parentAdj, Matrix6d & childAdj, Eigen::Vector3d & g) const 12 | { 13 | int numObj = (int)LS->blocks.size(); 14 | int row = SS->row; 15 | 16 | int parentIndex; 17 | int childIndex; 18 | if (!root) 19 | { 20 | parentIndex = LS->find(parentName)->joint->jindex; 21 | } 22 | childIndex = LS->find(childName)->joint->jindex; 23 | 24 | // Instead of adding three rows of constraints, we will need to add only two 25 | for (int r = 0; r < 2; ++r) // X is the free movement direction 26 | { 27 | for (int c = 0; c < 6; ++c) 28 | { 29 | if (!root) 30 | { 31 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, parentIndex * 6 + c, parentAdj(r + 4, c))); 32 | SS->LHSlist.push_back(Eigen::Triplet(parentIndex * 6 + c, numObj * 6 + row + r, parentAdj(r + 4, c))); 33 | } 34 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, childIndex * 6 + c, -1 * childAdj(r + 4, c))); 35 | SS->LHSlist.push_back(Eigen::Triplet(childIndex * 6 + c, numObj * 6 + row + r, -1 * childAdj(r + 4, c))); 36 | } 37 | // Load the corrective factor -1/h*g into f 38 | SS->fn[numObj * 6 + row + r] = g[r + 1]; 39 | } 40 | row = row + 2; 41 | 42 | SS->row = row; 43 | } 44 | 45 | void JointSlider::mapSelf(int c_i, std::unique_ptr& S) 46 | { 47 | } 48 | 49 | void JointSlider::updateSelf() 50 | { 51 | } 52 | -------------------------------------------------------------------------------- /c++/PCG/src/online/MatrixStack.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _MatrixStack_H_ 3 | #define _MatrixStack_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | class MatrixStack 10 | { 11 | public: 12 | MatrixStack(); 13 | virtual ~MatrixStack(); 14 | 15 | // glPushMatrix(): Copies the current matrix and adds it to the top of the stack 16 | void pushMatrix(); 17 | // glPopMatrix(): Removes the top of the stack and sets the current matrix to be the matrix that is now on top 18 | void popMatrix(); 19 | 20 | // glLoadIdentity(): Sets the top matrix to be the identity 21 | void loadIdentity(); 22 | // glMultMatrix(): Right multiplies the top matrix 23 | void multMatrix(const glm::mat4 &matrix); 24 | 25 | // glTranslate(): Right multiplies the top matrix by a translation matrix 26 | void translate(const glm::vec3 &trans); 27 | void translate(float x, float y, float z); 28 | // glScale(): Right multiplies the top matrix by a scaling matrix 29 | void scale(const glm::vec3 &scale); 30 | void scale(float x, float y, float z); 31 | // glScale(): Right multiplies the top matrix by a scaling matrix 32 | void scale(float size); 33 | // glRotate(): Right multiplies the top matrix by a rotation matrix (angle in radians) 34 | void rotate(float angle, const glm::vec3 &axis); 35 | void rotate(float angle, float x, float y, float z); 36 | 37 | // glGet(GL_MODELVIEW_MATRIX): Gets the top matrix 38 | const glm::mat4 &topMatrix() const; 39 | 40 | // Prints out the specified matrix 41 | static void print(const glm::mat4 &mat, const char *name = 0); 42 | // Prints out the top matrix 43 | void print(const char *name = 0) const; 44 | 45 | private: 46 | std::shared_ptr< std::stack > mstack; 47 | 48 | }; 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /matlab/+redmax/BodyCylinder.m: -------------------------------------------------------------------------------- 1 | classdef BodyCylinder < redmax.Body 2 | %BodyCylinder A cylinder along the Z axis 3 | % 4 | 5 | %% 6 | properties 7 | radius 8 | height 9 | end 10 | 11 | %% 12 | methods 13 | %% 14 | function this = BodyCylinder(density,radius,height) 15 | this = this@redmax.Body(density); 16 | this.radius = radius; 17 | this.height = height; 18 | end 19 | 20 | %% 21 | function computeInertia_(this) 22 | % Computes inertia at body 23 | r2 = this.radius*this.radius; 24 | h2 = this.height*this.height; 25 | a = pi*r2; 26 | v = this.height*a; 27 | m = v*this.density; 28 | Ixy = m/12.0*(3*r2 + h2); 29 | Iz = 0.5*m*r2; 30 | this.I_i = [Ixy Ixy Iz m m m]'; 31 | end 32 | 33 | %% 34 | function draw_(this) 35 | n = 8; 36 | [X,Y,Z] = cylinder(this.radius,n); 37 | Z = 0.5*(Z*2 - 1)*this.height; 38 | function [X,Y,Z] = xform(X,Y,Z) 39 | X = reshape(X,1,2*(n+1)); 40 | Y = reshape(Y,1,2*(n+1)); 41 | Z = reshape(Z,1,2*(n+1)); 42 | XYZ = this.E_wi*[X;Y;Z;ones(1,2*(n+1))]; 43 | X = reshape(XYZ(1,:),2,n+1); 44 | Y = reshape(XYZ(2,:),2,n+1); 45 | Z = reshape(XYZ(3,:),2,n+1); 46 | end 47 | [X,Y,Z] = xform(X,Y,Z); 48 | [T,R] = meshgrid(linspace(0,2*pi,n+1),linspace(0,this.radius,2)); 49 | X0 = R.*cos(T); 50 | Y0 = R.*sin(T); 51 | Z0 = -0.5*this.height*ones(size(X0)); 52 | [X_,Y_,Z_] = xform(X0,Y0,Z0); 53 | X = [X,X_]; 54 | Y = [Y,Y_]; 55 | Z = [Z,Z_]; 56 | Z0 = 0.5*this.height*ones(size(X0)); 57 | [X_,Y_,Z_] = xform(X0,Y0,Z0); 58 | X = [X,X_]; 59 | Y = [Y,Y_]; 60 | Z = [Z,Z_]; 61 | surf(X,Y,Z,'FaceColor',this.color); 62 | end 63 | 64 | %% 65 | function s = getAxisSize(this) 66 | s = 2*this.radius; 67 | end 68 | end 69 | end 70 | -------------------------------------------------------------------------------- /matlab/+redmax/ConstraintJointLimit.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintJointLimit < redmax.Constraint 2 | % ConstraintJointLimit Inequality joint limit constraint 3 | % Works only for revolute joints 4 | 5 | %% 6 | properties 7 | joint % Joint to apply the constraint on 8 | ql % lower limit 9 | qu % upper limit 10 | end 11 | 12 | %% 13 | methods 14 | %% 15 | function this = ConstraintJointLimit(joint) 16 | this = this@redmax.Constraint(0,0,0,1); 17 | this.name = [joint.name,'-LIMIT']; 18 | this.joint = joint; 19 | end 20 | 21 | %% 22 | function setLimits(this,ql,qu) 23 | this.ql = ql; 24 | this.qu = qu; 25 | end 26 | end 27 | 28 | %% 29 | methods (Access = protected) 30 | %% 31 | function [Cr,Crdot,cr,crdot,crddot] = computeJacIneqR_(this,Cr,Crdot,cr,crdot,crddot) 32 | rows = this.idxIR; 33 | cols = this.joint.idxR; 34 | this.idxQ = cols; 35 | if this.joint.q(1) <= this.ql 36 | Cr(rows,cols) = -1; 37 | cr(rows) = this.ql - this.joint.q(1); 38 | this.activeR = true; 39 | elseif this.joint.q(1) >= this.qu 40 | Cr(rows,cols) = 1; 41 | cr(rows) = this.qu - this.joint.q(1); 42 | this.activeR = true; 43 | else 44 | this.activeR = false; 45 | end 46 | end 47 | 48 | %% 49 | function [value,isterminal,direction] = ineqEventFcn_(this,value,isterminal,direction) 50 | q = this.joint.q(1); 51 | value(end+1) = q - this.ql; 52 | isterminal(end+1) = 1; 53 | direction(end+1) = -1; 54 | value(end+1) = q - this.qu; 55 | isterminal(end+1) = 1; 56 | direction(end+1) = 1; 57 | end 58 | 59 | %% 60 | function ineqProjPos_(this) 61 | if this.joint.q(1) <= this.ql 62 | this.joint.q(1) = this.ql; 63 | elseif this.joint.q(1) >= this.qu 64 | this.joint.q(1) = this.qu; 65 | end 66 | end 67 | end 68 | end 69 | -------------------------------------------------------------------------------- /matlab/+redmax/ConstraintPrescJointM.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintPrescJointM < redmax.Constraint 2 | % ConstraintPrescJointM Prescribes body motion in maximal coords 3 | % Only works with revolute joints and with velocity-level Euler 4 | 5 | %% 6 | properties 7 | joint 8 | vel % velocity-level integrator? 9 | q 10 | qdot 11 | qddot 12 | end 13 | 14 | %% 15 | methods 16 | %% 17 | function this = ConstraintPrescJointM(joint,vel) 18 | this = this@redmax.Constraint(1,0,0,0); 19 | this.name = [joint.name,'-presc']; 20 | this.joint = joint; 21 | this.vel = vel; 22 | this.q = 0; 23 | this.qdot = 0; 24 | this.qddot = 0; 25 | end 26 | end 27 | 28 | %% 29 | methods (Access = protected) 30 | %% 31 | function [Gm,Gmdot,gm,gmdot,gmddot] = computeJacEqM_(this,Gm,Gmdot,gm,gmdot,gmddot) 32 | % Assumes both bodies exist 33 | rows = this.idxEM; 34 | colsI = this.joint.body.idxM; 35 | colsP = this.joint.parent.body.idxM; 36 | this.idxQ = [colsI colsP]; 37 | Ad_ji = this.joint.body.Ad_ji; 38 | Ad_iw = this.joint.body.Ad_iw; 39 | Ad_wp = this.joint.parent.body.Ad_wi; 40 | GmI = Ad_ji; 41 | GmP = -Ad_ji*Ad_iw*Ad_wp; 42 | if sum(this.joint.axis == [1 0 0]') == 3 43 | % Revolute around X-axis 44 | Gm(rows,colsI) = GmI(1,:); 45 | Gm(rows,colsP) = GmP(1,:); 46 | elseif sum(this.joint.axis == [0 1 0]') == 3 47 | % Revolute around Y-axis 48 | Gm(rows,colsI) = GmI(2,:); 49 | Gm(rows,colsP) = GmP(2,:); 50 | elseif sum(this.joint.axis == [0 0 1]') == 3 51 | % Revolute around Z-axis 52 | Gm(rows,colsI) = GmI(3,:); 53 | Gm(rows,colsP) = GmP(3,:); 54 | end 55 | if this.vel 56 | gmdot(rows) = this.qdot; % Y-axis rotation 57 | else 58 | error('only works with velocity-level Euler'); 59 | end 60 | end 61 | end 62 | end 63 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/ForceSpringDamper.m: -------------------------------------------------------------------------------- 1 | classdef ForceSpringDamper < redmax.ForceSpringGeneric 2 | %ForceSpringDamper A damped spring force between two points 3 | 4 | %% 5 | properties 6 | L 7 | stiffness 8 | damping 9 | end 10 | 11 | methods 12 | function this = ForceSpringDamper(body1,x_1,body2,x_2) 13 | this = this@redmax.ForceSpringGeneric(body1,x_1,body2,x_2); 14 | this.stiffness = 1; 15 | this.damping = 1; 16 | this.L = 0; 17 | end 18 | 19 | %% 20 | function setStiffness(this,stiffness) 21 | this.stiffness = stiffness; 22 | end 23 | 24 | %% 25 | function setDamping(this,damping) 26 | this.damping = damping; 27 | end 28 | 29 | %% 30 | function setRetLength(this,L) 31 | this.L = L; 32 | end 33 | end 34 | 35 | methods (Access = protected) 36 | %% 37 | function init_(this) 38 | % Compute rest length, unless it has been set manually 39 | if this.L > 0 40 | return; 41 | end 42 | E1 = eye(4); 43 | E2 = eye(4); 44 | xl1 = this.x_1; 45 | xl2 = this.x_2; 46 | if ~isempty(this.body1) 47 | E1 = this.body1.E_wi; 48 | end 49 | if ~isempty(this.body2) 50 | E2 = this.body2.E_wi; 51 | end 52 | R1 = E1(1:3,1:3); 53 | R2 = E2(1:3,1:3); 54 | p1 = E1(1:3,4); 55 | p2 = E2(1:3,4); 56 | xw1 = R1*xl1 + p1; 57 | xw2 = R2*xl2 + p2; 58 | dx = xw2 - xw1; 59 | this.L = norm(dx); 60 | end 61 | 62 | %% 63 | function [V,f,dfdl,dfdldot] = computeSpringForce(this,l,ldot) 64 | % If the force is positive, the spring will try to contract 65 | strain = (l-this.L)/this.L; 66 | dstrain = ldot/this.L; 67 | V = (this.stiffness/2)*strain^2*this.L; 68 | f = this.stiffness*strain + this.damping*dstrain; 69 | dfdl = this.stiffness/this.L; 70 | dfdldot = this.damping/this.L; 71 | end 72 | end 73 | end 74 | -------------------------------------------------------------------------------- /c++/PCG/src/JointPrismatic.cpp: -------------------------------------------------------------------------------- 1 | #include "JointHinge.h" 2 | 3 | #include "Block.h" 4 | #include "LinkageSystem.h" 5 | #include "State.h" 6 | #include "StateDeriv.h" 7 | #include "StateSolve.h" 8 | #include "JointPrismatic.h" 9 | 10 | JointPrismatic::JointPrismatic(std::unique_ptr& S, std::string pn, std::string cn, Eigen::Vector3d pp, Eigen::Vector3d cp, bool r, int i, double angle, Eigen::Vector3d & axis, double k, double d) : 11 | Joint(S, JType::Prismatic, pn, cn, pp, cp, r, i, 1, k, d) 12 | { 13 | this->axis = axis; 14 | 15 | this->S = Eigen::MatrixXd::Zero(6, 1); 16 | this->S.block<3, 1>(3, 0) = axis; 17 | this->ST = this->S.transpose(); 18 | Sdot = Eigen::MatrixXd::Zero(6, 1); 19 | 20 | q0 = Eigen::VectorXd(constraintNum); 21 | q0[0] = angle; 22 | 23 | Q = Eigen::Matrix4d::Identity(); 24 | 25 | Pr = Eigen::MatrixXd::Zero(1, 1); 26 | Psi = Eigen::MatrixXd::Zero(1, 1); 27 | Kmd = Matrix6d::Zero(); 28 | Dmd = Matrix6d::Zero(); 29 | } 30 | 31 | void JointPrismatic::updateMaxSparse(std::unique_ptr& SS, const std::unique_ptr& LS, const std::unique_ptr& S, Matrix6d & parentAdj, Matrix6d & childAdj, Eigen::Vector3d & g) const 32 | { 33 | } 34 | 35 | void JointPrismatic::mapSelf(int c_i, std::unique_ptr& S) 36 | { 37 | new (&q) Eigen::Map(&(S->q[c_i]), constraintNum); 38 | q[0] = q0[0]; 39 | 40 | new (&qdot) Eigen::Map(&(S->qdot[c_i]), constraintNum); 41 | qdot = Eigen::VectorXd::Zero(constraintNum); 42 | 43 | updateSelf(); 44 | } 45 | 46 | void JointPrismatic::updateSelf() 47 | { 48 | Eigen::Vector3d t_wj0; 49 | if (q.size() != 0) 50 | t_wj0 = axis * q[0]; 51 | else 52 | t_wj0 = axis * q0[0]; 53 | Q.block<3, 1>(0, 3) = t_wj0; 54 | } 55 | 56 | Vector6d JointPrismatic::Sqdot(const std::unique_ptr& S) 57 | { 58 | return this->S * qdot; 59 | } 60 | -------------------------------------------------------------------------------- /c++/PCG/cmake/FindPardiso.cmake: -------------------------------------------------------------------------------- 1 | # Find pardiso 2 | # 3 | # Find the pardiso library 4 | # 5 | # if you need to add a custom library search path, do it via CMAKE_PREFIX_PATH 6 | # 7 | # This module defines 8 | # PARDISO_LIBRARIES, the libraries needed to use PARDISO. 9 | # PARDISO_FOUND, If false, do not try to use PARDISO. 10 | # PARDISO_INCLUDE_PREFIX, include prefix for PARDISO 11 | 12 | # Additional modules 13 | include(FindPackageHandleStandardArgs) 14 | 15 | IF(NOT PARDISO_ROOT_DIR) 16 | SET(PARDISO_ROOT_DIR "$ENV{PARDISO_ROOT_DIR}") 17 | ENDIF() 18 | 19 | if (WIN32) 20 | # Find library files 21 | find_library( 22 | PARDISO_LIBRARY 23 | NAMES libpardiso600-WIN-X86-64 24 | PATHS 25 | $ENV{PROGRAMFILES}/lib/ 26 | ${PARDISO_ROOT_DIR}/) 27 | elseif(APPLE) 28 | # Find library files 29 | # Try to use static libraries 30 | find_library( 31 | PARDISO_LIBRARY 32 | NAMES pardiso600-MACOS-X86-64 33 | PATHS 34 | /usr/lib64/ 35 | /usr/lib/ 36 | /usr/local/lib64/ 37 | /usr/local/lib/ 38 | /sw/lib/ 39 | /opt/local/lib/ 40 | ${PARDISO_ROOT_DIR}/ 41 | ${PARDISO_ROOT_DIR}/lib/ 42 | ${PARDISO_ROOT_DIR}/src/ 43 | DOC "The PARDISO library") 44 | else() 45 | # Find library files 46 | # Try to use static libraries 47 | find_library( 48 | PARDISO_LIBRARY 49 | NAMES pardiso600-MACOS-X86-64 50 | PATHS 51 | /usr/lib64/ 52 | /usr/lib/ 53 | /usr/local/lib64/ 54 | /usr/local/lib/ 55 | /sw/lib/ 56 | /opt/local/lib/ 57 | ${PARDISO_ROOT_DIR}/ 58 | ${PARDISO_ROOT_DIR}/lib/ 59 | ${PARDISO_ROOT_DIR}/src/ 60 | DOC "The PARDISO library") 61 | endif() 62 | 63 | # Handle REQUIRD argument, define *_FOUND variable 64 | find_package_handle_standard_args(PARDISO DEFAULT_MSG PARDISO_LIBRARY) 65 | if (PARDISO_FOUND) 66 | set(PARDISO_LIBRARIES ${PARDISO_LIBRARY}) 67 | endif() 68 | 69 | # Hide some variables 70 | mark_as_advanced (PARDISO_LIBRARY) -------------------------------------------------------------------------------- /matlab/+redmax/JointComposite.m: -------------------------------------------------------------------------------- 1 | classdef JointComposite < redmax.Joint 2 | % https://github.com/junggon/gear/blob/master/src/gjoint_composite.cpp 3 | 4 | %% 5 | properties 6 | joint1 7 | joint2 8 | end 9 | 10 | %% 11 | methods 12 | %% 13 | function this = JointComposite(parent,body,joint1,joint2) 14 | this = this@redmax.Joint(parent,body,joint1.ndof+joint2.ndof); 15 | this.joint1 = joint1; 16 | this.joint2 = joint2; 17 | body.joint = this; 18 | end 19 | end 20 | 21 | %% 22 | methods (Access = protected) 23 | %% 24 | function reparam_(this) 25 | ndof1 = this.joint1.ndof; 26 | ndof2 = this.joint2.ndof; 27 | idx1 = 1:ndof1; 28 | idx2 = ndof1+1:ndof1+ndof2; 29 | this.joint1.reparam_(); 30 | this.joint2.reparam_(); 31 | this.q(idx1) = this.joint1.q; 32 | this.q(idx2) = this.joint2.q; 33 | this.qdot(idx1) = this.joint1.qdot; 34 | this.qdot(idx2) = this.joint2.qdot; 35 | end 36 | 37 | %% 38 | function update_(this) 39 | ndof1 = this.joint1.ndof; 40 | ndof2 = this.joint2.ndof; 41 | idx1 = 1:ndof1; 42 | idx2 = ndof1+1:ndof1+ndof2; 43 | this.joint1.q = this.q(idx1); 44 | this.joint2.q = this.q(idx2); 45 | this.joint1.qdot = this.qdot(idx1); 46 | this.joint2.qdot = this.qdot(idx2); 47 | this.joint1.update_(); 48 | this.joint2.update_(); 49 | 50 | Q1 = this.joint1.Q; 51 | Q2 = this.joint2.Q; 52 | this.Q = Q1*Q2; 53 | 54 | S1 = this.joint1.S; 55 | S2 = this.joint2.S; 56 | Ad_21 = se3.Ad(se3.inv(Q2)); 57 | this.S(1:6,idx1) = Ad_21*S1; 58 | this.S(1:6,idx2) = S2; 59 | 60 | dS1 = this.joint1.Sdot; 61 | dS2 = this.joint2.Sdot; 62 | Sdq2 = S2*this.joint2.qdot; 63 | this.Sdot(1:6,idx1) = -se3.ad(Sdq2)*Ad_21*S1 + Ad_21*dS1; 64 | this.Sdot(1:6,idx2) = dS2; 65 | end 66 | 67 | %% 68 | function draw_(this) %#ok 69 | % TODO 70 | end 71 | end 72 | end 73 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/JointFree2D.m: -------------------------------------------------------------------------------- 1 | classdef JointFree2D < redmax.Joint 2 | % 2D free joint in XY 3 | % 4 | 5 | %% 6 | properties 7 | end 8 | 9 | %% 10 | methods 11 | %% 12 | function this = JointFree2D(parent,body) 13 | this = this@redmax.Joint(parent,body,3); 14 | end 15 | end 16 | 17 | %% 18 | methods (Access = protected) 19 | %% 20 | function update_(this,deriv) 21 | n = this.ndof; 22 | p = [this.q(1:2); 0]; 23 | r = this.q(3); 24 | pdot = this.qdot(1:2); 25 | rdot = this.qdot(3); 26 | R = eye(3); 27 | s = sin(r); 28 | c = cos(r); 29 | R(1:2,1:2) = [c -s; s c]; 30 | this.Q = [R p; 0 0 0 1]; 31 | this.A = se3.Ad(this.Q); 32 | this.S(3,3) = 1; 33 | this.S(4:5,1:2) = [c s; -s c]; 34 | 35 | pbrac = se3.brac(p); 36 | dRdq = zeros(3); 37 | dRdq(1:2,1:2) = [-s -c; c -s]; 38 | 39 | pdotbrac = se3.brac([pdot;0]); 40 | Rdot = dRdq*rdot; 41 | this.Adot(1:3,1:3) = Rdot; 42 | this.Adot(4:6,4:6) = Rdot; 43 | this.Adot(4:6,1:3) = pdotbrac*R + pbrac*Rdot; 44 | this.Sdot(4:5,1:2) = [-s, c; -c, -s]*rdot; 45 | 46 | if deriv 47 | this.dSdotdq(4:5,1:2,3) = [-c, -s; s, -c]*rdot; 48 | 49 | e1brac = se3.brac([1 0 0]); 50 | e2brac = se3.brac([0 1 0]); 51 | this.dAdq(4:6,1:3,1) = e1brac*R; 52 | this.dAdq(4:6,1:3,2) = e2brac*R; 53 | this.dAdq(1:3,1:3,3) = dRdq; 54 | this.dAdq(4:6,1:3,3) = pbrac*dRdq; 55 | this.dAdq(4:6,4:6,3) = dRdq; 56 | 57 | this.dSdq = zeros(6,3,n); 58 | this.dSdq(4:5,1:2,3) = [-s c; -c -s]; 59 | 60 | dRdotdq = zeros(3,3); 61 | dRdotdq(1:2,1:2) = [-c, s; -s, -c]*rdot; 62 | this.dAdotdq(4:6,1:3,1) = e1brac*Rdot; 63 | this.dAdotdq(4:6,1:3,2) = e2brac*Rdot; 64 | this.dAdotdq(1:3,1:3,3) = dRdotdq; 65 | this.dAdotdq(4:6,4:6,3) = dRdotdq; 66 | this.dAdotdq(4:6,1:3,3) = pdotbrac*dRdq + pbrac*dRdotdq; 67 | end 68 | end 69 | end 70 | end 71 | -------------------------------------------------------------------------------- /matlab/+redmax/ConstraintAttachSpring.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintAttachSpring < redmax.Constraint 2 | % ConstraintAttachSpring Attaches the two ends of a string to rigid 3 | % bodies 4 | 5 | %% 6 | properties 7 | spring 8 | end 9 | 10 | %% 11 | methods 12 | %% 13 | function this = ConstraintAttachSpring(spring) 14 | this = this@redmax.Constraint(6,0,0,0); 15 | this.name = [spring.name,'-A']; 16 | this.spring = spring; 17 | end 18 | end 19 | 20 | %% 21 | methods (Access = protected) 22 | %% 23 | function [Gm,Gmdot,gm,gmdot,gmddot] = computeJacEqM_(this,Gm,Gmdot,gm,gmdot,gmddot) 24 | % g = E*r - x 25 | % G = [R*Gamma, -I] 26 | % Gdot = [R*[w]*Gamma, 0] 27 | I = eye(3); 28 | rows0 = this.idxEM(1:3); 29 | rows1 = this.idxEM(4:6); 30 | cols0S = this.spring.nodes(1).idxM; 31 | cols1S = this.spring.nodes(end).idxM; 32 | body0 = this.spring.body0; 33 | body1 = this.spring.body1; 34 | if isempty(body0) 35 | E0 = eye(4); 36 | cols0B = []; 37 | else 38 | E0 = body0.E_wi; 39 | cols0B = body0.idxM; 40 | end 41 | if isempty(body1) 42 | E1 = eye(4); 43 | cols1B = []; 44 | else 45 | E1 = body1.E_wi; 46 | cols1B = body1.idxM; 47 | end 48 | R0 = E0(1:3,1:3); 49 | R1 = E1(1:3,1:3); 50 | G0 = se3.Gamma(this.spring.r0); 51 | G1 = se3.Gamma(this.spring.r1); 52 | if ~isempty(body0) 53 | W0 = se3.brac(body0.phi(1:3)); 54 | Gm(rows0,cols0B) = R0*G0; 55 | Gmdot(rows0,cols0B) = R0*W0*G0; 56 | end 57 | if ~isempty(body1) 58 | W1 = se3.brac(body1.phi(1:3)); 59 | Gm(rows1,cols1B) = R1*G1; 60 | Gmdot(rows1,cols1B) = R1*W1*G1; 61 | end 62 | Gm(rows0,cols0S) = -I; 63 | Gm(rows1,cols1S) = -I; 64 | gm0 = E0*[this.spring.r0;1] - [this.spring.nodes( 1).x;1]; 65 | gm1 = E1*[this.spring.r1;1] - [this.spring.nodes(end).x;1]; 66 | gm(rows0) = gm0(1:3,1); 67 | gm(rows1) = gm1(1:3,1); 68 | end 69 | end 70 | end 71 | -------------------------------------------------------------------------------- /c++/PCG/cmake/FindMKL.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Find MKL 3 | # 4 | # Try to find MKL library. 5 | # This module defines the following variables: 6 | # - MKL_INCLUDE_DIRS 7 | # - MKL_LIBRARIES 8 | # - MKL_FOUND 9 | # 10 | # The following variables can be set as arguments for the module. 11 | # - MKL_ROOT_DIR : Root library directory of MKL 12 | 13 | # Additional modules 14 | include(FindPackageHandleStandardArgs) 15 | 16 | if (WIN32) 17 | # Find include files 18 | find_path( 19 | MKL_INCLUDE_DIR 20 | NAMES mkl.h 21 | PATHS 22 | $ENV{PROGRAMFILES}/include/ 23 | ${MKL_ROOT_DIR}/include/ 24 | DOC "The directory where MKL.h resides") 25 | 26 | # Find library files 27 | find_library( 28 | MKL_LIBRARY 29 | NAMES mkl_core mkl_intel_lp64 mkl_sequential 30 | PATHS 31 | $ENV{PROGRAMFILES}/lib/ 32 | ${MKL_ROOT_DIR}/lib/intel64_win/ 33 | ${MKL_ROOT_DIR}/src/) 34 | else() 35 | # Find include files 36 | find_path( 37 | MKL_INCLUDE_DIR 38 | NAMES mkl.h 39 | PATHS 40 | /usr/include/ 41 | /usr/local/include/ 42 | /sw/include/ 43 | /opt/local/include/ 44 | ${MKL_ROOT_DIR}/include/ 45 | DOC "The directory where MKL.h resides") 46 | 47 | # Find library files 48 | # Try to use static libraries 49 | find_library( 50 | MKL_LIBRARY 51 | NAMES mkl_core mkl_intel_lp64 mkl_sequential 52 | PATHS 53 | /usr/lib64/ 54 | /usr/lib/ 55 | /usr/local/lib64/ 56 | /usr/local/lib/ 57 | /sw/lib/ 58 | /opt/local/lib/ 59 | ${MKL_ROOT_DIR}/lib/ 60 | ${MKL_ROOT_DIR}/lib/intel64/ 61 | ${MKL_ROOT_DIR}/src/ 62 | DOC "The MKL library") 63 | endif() 64 | 65 | # Handle REQUIRD argument, define *_FOUND variable 66 | find_package_handle_standard_args(MKL DEFAULT_MSG MKL_INCLUDE_DIR MKL_LIBRARY) 67 | 68 | # Define MKL_LIBRARIES and MKL_INCLUDE_DIRS 69 | if (MKL_FOUND) 70 | set(MKL_LIBRARIES ${OPENGL_LIBRARIES} ${MKL_LIBRARY}) 71 | set(MKL_INCLUDE_DIRS ${MKL_INCLUDE_DIR}) 72 | endif() 73 | 74 | # Hide some variables 75 | mark_as_advanced(MKL_INCLUDE_DIR MKL_LIBRARY) -------------------------------------------------------------------------------- /matlab/+redmax/JointFree.m: -------------------------------------------------------------------------------- 1 | classdef JointFree < redmax.Joint 2 | % https://github.com/junggon/gear/blob/master/src/gjoint_free.cpp 3 | % Copied from GJointFreeST 4 | % 5 | 6 | %% 7 | properties 8 | jointS 9 | jointT 10 | end 11 | 12 | %% 13 | methods 14 | %% 15 | function this = JointFree(parent,body) 16 | this = this@redmax.Joint(parent,body,6); 17 | this.jointS = redmax.JointSphericalExp([],body); 18 | this.jointT = redmax.JointTranslational([],body); 19 | body.joint = this; 20 | end 21 | end 22 | 23 | %% 24 | methods (Access = protected) 25 | %% 26 | function reparam_(this) 27 | this.jointS.reparam_(); 28 | this.q(1:3) = this.jointS.q; 29 | this.qdot(1:3) = this.jointS.qdot; 30 | end 31 | 32 | %% 33 | function update_(this) 34 | this.jointS.q = this.q(1:3); 35 | this.jointT.q = this.q(4:6); 36 | this.jointS.qdot = this.qdot(1:3); 37 | this.jointT.qdot = this.qdot(4:6); 38 | this.jointS.update_(); 39 | this.jointT.update_(); 40 | 41 | % T1 = spherical_joint.T = SE3(R, 0) 42 | % T2 = translational_joint.T = SE3(eye, p) 43 | % T = T1*T2 = SE3(R, R*p) 44 | Q1 = this.jointS.Q; 45 | Q2 = this.jointT.Q; 46 | this.Q = Q1*Q2; 47 | 48 | % S.Push(0, 0, S1w); 49 | % S.Push(3, 0, -Cross(p, S1w)); 50 | % S.Push(0, 3, translational_joint.S); 51 | S1 = this.jointS.S; 52 | S2 = this.jointT.S; 53 | S1w = S1(1:3,1:3); 54 | p = Q2(1:3,4); 55 | pbrac = se3.brac(p); 56 | this.S(1:3,1:3) = S1w; 57 | this.S(4:6,1:3) = -pbrac*S1w; 58 | this.S(1:6,4:6) = S2; 59 | 60 | % dS.Push(0, 0, dS1w); 61 | % dS.Push(3, 0, -(Cross(S2dq2v, S1w) + Cross(p, dS1w))); 62 | % dS.Push(0, 3, translational_joint.dS); 63 | dS1 = this.jointS.Sdot; 64 | dS2 = this.jointT.Sdot; 65 | dS1w = dS1(1:3,1:3); 66 | this.Sdot(1:3,1:3) = dS1w; 67 | this.Sdot(4:6,1:3) = -(se3.brac(this.qdot(4:6))*S1w + pbrac*dS1w); 68 | this.Sdot(1:6,4:6) = dS2; 69 | end 70 | end 71 | end 72 | -------------------------------------------------------------------------------- /c++/PCG/src/online/Camera.cpp: -------------------------------------------------------------------------------- 1 | #define _USE_MATH_DEFINES 2 | #include 3 | #include "Camera.h" 4 | #include "MatrixStack.h" 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | Camera::Camera() : 11 | aspect(1.0f), 12 | fovy((float)(45.0*M_PI/180.0)), 13 | znear(0.1f), 14 | zfar(1000.0f), 15 | rotations(0.0, 0.0), 16 | translations(0.0f, 9.0f, -24.8f), 17 | rfactor(0.01f), 18 | tfactor(0.001f), 19 | sfactor(0.005f) 20 | { 21 | } 22 | 23 | Camera::~Camera() 24 | { 25 | } 26 | 27 | void Camera::mouseClicked(const float x, const float y, const bool shift, const bool ctrl, const bool alt) 28 | { 29 | mousePrev.x = x; 30 | mousePrev.y = y; 31 | if(shift) { 32 | state = Camera::TRANSLATE; 33 | } else if(ctrl) { 34 | state = Camera::SCALE; 35 | } else { 36 | state = Camera::ROTATE; 37 | } 38 | } 39 | 40 | void Camera::mouseMoved(const float x, const float y) 41 | { 42 | glm::vec2 mouseCurr(x, y); 43 | glm::vec2 dv = mouseCurr - mousePrev; 44 | switch(state) { 45 | case Camera::ROTATE: 46 | rotations += rfactor * dv; 47 | break; 48 | case Camera::TRANSLATE: 49 | translations.x -= translations.z * tfactor * dv.x; 50 | translations.y += translations.z * tfactor * dv.y; 51 | break; 52 | case Camera::SCALE: 53 | translations.z *= (1.0f - sfactor * dv.y); 54 | break; 55 | } 56 | mousePrev = mouseCurr; 57 | } 58 | 59 | void Camera::applyProjectionMatrix(const std::unique_ptr &P) const 60 | { 61 | // Modify provided MatrixStack 62 | P->multMatrix(glm::perspective(fovy, aspect, znear, zfar)); 63 | } 64 | 65 | void Camera::applyViewMatrix(const std::unique_ptr &MV) const 66 | { 67 | MV->translate(translations); 68 | MV->rotate(rotations.y, glm::vec3(1.0f, 0.0f, 0.0f)); 69 | MV->rotate(rotations.x, glm::vec3(0.0f, 1.0f, 0.0f)); 70 | } 71 | 72 | void Camera::setTranslations(const float x, const float y, const float z) 73 | { 74 | translations = glm::vec3(x, y, z); 75 | } 76 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/ForceCable.m: -------------------------------------------------------------------------------- 1 | classdef ForceCable < redmax.ForceSpringMultiPointGeneric 2 | %ForceCable A cable routed through multiple points 3 | 4 | %% 5 | properties 6 | L 7 | stiffness 8 | damping 9 | end 10 | 11 | methods 12 | function this = ForceCable() 13 | this = this@redmax.ForceSpringMultiPointGeneric(); 14 | this.stiffness = 1; 15 | this.damping = 1; 16 | this.L = 0; 17 | end 18 | 19 | %% 20 | function setStiffness(this,stiffness) 21 | this.stiffness = stiffness; 22 | end 23 | 24 | %% 25 | function setDamping(this,damping) 26 | this.damping = damping; 27 | end 28 | 29 | %% 30 | function setRetLength(this,L) 31 | this.L = L; 32 | end 33 | end 34 | 35 | methods (Access = protected) 36 | %% 37 | function init_(this) 38 | % Compute rest length, unless it has been set manually 39 | if this.L > 0 40 | return; 41 | end 42 | this.L = 0; 43 | npts = length(this.bodies); 44 | for k = 1 : npts - 1 45 | E1 = eye(4); 46 | E2 = eye(4); 47 | xl1 = this.xls{k}; 48 | xl2 = this.xls{k+1}; 49 | if ~isempty(this.bodies{k}) 50 | E1 = this.bodies{k}.E_wi; 51 | end 52 | if ~isempty(this.bodies{k+1}) 53 | E2 = this.bodies{k+1}.E_wi; 54 | end 55 | R1 = E1(1:3,1:3); 56 | R2 = E2(1:3,1:3); 57 | p1 = E1(1:3,4); 58 | p2 = E2(1:3,4); 59 | xw1 = R1*xl1 + p1; 60 | xw2 = R2*xl2 + p2; 61 | dx = xw2 - xw1; 62 | this.L = this.L + norm(dx); 63 | end 64 | end 65 | 66 | %% 67 | function [V,f,dfdl,dfdldot] = computeSpringForce(this,l,ldot) 68 | % If the force is positive, the spring will try to contract 69 | strain = (l - this.L)/this.L; 70 | dstrain = ldot/this.L; 71 | if strain > 0 72 | V = (this.stiffness/2)*strain^2*this.L; 73 | f = this.stiffness*strain + this.damping*dstrain; 74 | dfdl = this.stiffness/this.L; 75 | dfdldot = this.damping/this.L; 76 | else 77 | V = 0; 78 | f = 0; 79 | dfdl = 0; 80 | dfdldot = 0; 81 | end 82 | end 83 | end 84 | end 85 | -------------------------------------------------------------------------------- /c++/PCG/cmake/FindGLEW.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Find GLEW 3 | # 4 | # Try to find GLEW library. 5 | # This module defines the following variables: 6 | # - GLEW_INCLUDE_DIRS 7 | # - GLEW_LIBRARIES 8 | # - GLEW_FOUND 9 | # 10 | # The following variables can be set as arguments for the module. 11 | # - GLEW_ROOT_DIR : Root library directory of GLEW 12 | 13 | # Additional modules 14 | include(FindPackageHandleStandardArgs) 15 | 16 | if (WIN32) 17 | # Find include files 18 | find_path( 19 | GLEW_INCLUDE_DIR 20 | NAMES GL/glew.h 21 | PATHS 22 | $ENV{PROGRAMFILES}/include/ 23 | ${GLEW_ROOT_DIR}/include/ 24 | DOC "The directory where GLEW/GLEW.h resides") 25 | 26 | # Find library files 27 | find_library( 28 | GLEW_LIBRARY 29 | NAMES glew32s 30 | PATHS 31 | $ENV{PROGRAMFILES}/lib/ 32 | ${GLEW_ROOT_DIR}/lib/ 33 | ${GLEW_ROOT_DIR}/lib/Release/x64/ 34 | ${GLEW_ROOT_DIR}/lib/Debug/x64/ 35 | ${GLEW_ROOT_DIR}/src/ 36 | ${GLEW_ROOT_DIR}/src/Release/ 37 | ${GLEW_ROOT_DIR}/src/Debug/) 38 | else() 39 | # Find include files 40 | find_path( 41 | GLEW_INCLUDE_DIR 42 | NAMES GL/glew.h 43 | PATHS 44 | /usr/include/ 45 | /usr/local/include/ 46 | /sw/include/ 47 | /opt/local/include/ 48 | ${GLEW_ROOT_DIR}/include/ 49 | DOC "The directory where GL/GLEW.h resides") 50 | 51 | # Find library files 52 | # Try to use static libraries 53 | find_library( 54 | GLEW_LIBRARY 55 | NAMES GLEW libglew.so libGLEW.so 56 | PATHS 57 | /usr/lib64/ 58 | /usr/lib/ 59 | /usr/local/lib64/ 60 | /usr/local/lib/ 61 | /sw/lib/ 62 | /opt/local/lib/ 63 | ${GLEW_ROOT_DIR}/lib/ 64 | ${GLEW_ROOT_DIR}/src/ 65 | DOC "The GLEW library") 66 | endif() 67 | 68 | # Handle REQUIRD argument, define *_FOUND variable 69 | find_package_handle_standard_args(GLEW DEFAULT_MSG GLEW_INCLUDE_DIR GLEW_LIBRARY) 70 | 71 | # Define GLEW_LIBRARIES and GLEW_INCLUDE_DIRS 72 | if (GLEW_FOUND) 73 | set(GLEW_LIBRARIES ${OPENGL_LIBRARIES} ${GLEW_LIBRARY}) 74 | set(GLEW_INCLUDE_DIRS ${GLEW_INCLUDE_DIR}) 75 | endif() 76 | 77 | # Hide some variables 78 | mark_as_advanced(GLEW_INCLUDE_DIR GLEW_LIBRARY) -------------------------------------------------------------------------------- /c++/PCG/src/StateSolve.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define EIGEN_DONT_ALIGN_STATICALLY 4 | #include 5 | #include 6 | 7 | #include "Rigid.h" 8 | 9 | struct StateSolve 10 | { 11 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 12 | 13 | enum matrixSolveType { dense, sparse }; 14 | 15 | // define the simulation timestep 16 | double h; 17 | // define the damping alpha term 18 | double alpha; 19 | // environment gravity 20 | Eigen::Vector3d grav; 21 | // baumgarte 22 | Eigen::Vector3d baumgarte; 23 | // toggle sparse or dense 24 | matrixSolveType mst; 25 | 26 | // store the constraint matrix 27 | std::vector> LHSlist; 28 | // stiffness and damping 29 | std::vector> Kmlist; // joint 30 | std::vector> Dmlist; // joint 31 | std::vector> bDmlist; // body 32 | // fnew vector, holds Mv + hf, RHS of Ax = b for maximal coords 33 | Eigen::VectorXd fn; 34 | // only the force acting on each rigid body 35 | Eigen::VectorXd f; 36 | 37 | // stiffness and damping 38 | std::vector> Krlist; 39 | std::vector> Drlist; 40 | // jacobian matrix 41 | Eigen::MatrixXd J; // for building 42 | std::vector> Jlist; 43 | std::vector> JTlist; 44 | // and its time derivative 45 | Eigen::MatrixXd Jd; // for building 46 | std::vector> Jdlist; 47 | // reduced force 48 | Eigen::VectorXd fr; 49 | // additional maximal constraints 50 | std::vector> Gmlist; 51 | std::vector> GmTlist; 52 | std::vector> LHSGlist; 53 | Eigen::VectorXd gm; 54 | Eigen::MatrixXd Gm; // for CG 55 | Eigen::MatrixXd GmT; // for CG 56 | 57 | // permanents 58 | Eigen::SparseMatrix Kr; 59 | Eigen::SparseMatrix Dr; 60 | Eigen::SparseMatrix bDm; 61 | 62 | // track the number of constraints 63 | int row; 64 | 65 | // quick info about the system 66 | int Mm_dimension; 67 | int Mr_dimension; 68 | int Gm_dimension; 69 | int Gr_dimension; 70 | 71 | // for debugging solves 72 | //Eigen::MatrixXd LHS; 73 | }; -------------------------------------------------------------------------------- /matlab-diff/+redmax/Force.m: -------------------------------------------------------------------------------- 1 | classdef (Abstract) Force < handle 2 | %Force Massless force acting on bodies 3 | 4 | %% 5 | properties 6 | name % optional name 7 | next % Next force in traversal order 8 | end 9 | 10 | %% 11 | methods 12 | %% 13 | function this = Force() 14 | end 15 | 16 | %% 17 | function init(this) 18 | this.init_(); 19 | % Go to the next force 20 | if ~isempty(this.next) 21 | this.next.init(); 22 | end 23 | end 24 | 25 | %% 26 | function [fr,fm,Kr,Km,Dr,Dm] = computeValues(this,fr,fm,Kr,Km,Dr,Dm) 27 | nr = redmax.Scene.countR(); 28 | nm = redmax.Scene.countM(); 29 | if nargout == 2 30 | % Just the forces 31 | if nargin == 1 32 | fr = zeros(nr,1); 33 | fm = zeros(nm,1); 34 | end 35 | [fr,fm] = this.computeValues_(fr,fm); 36 | % Go to the next force 37 | if ~isempty(this.next) 38 | [fr,fm] = this.next.computeValues(fr,fm); 39 | end 40 | else 41 | % Forces and derivatives 42 | if nargin == 1 43 | fr = zeros(nr,1); 44 | fm = zeros(nm,1); 45 | Kr = zeros(nr); 46 | Km = zeros(nm); 47 | Dr = zeros(nr); 48 | Dm = zeros(nm); 49 | end 50 | [fr,fm,Kr,Km,Dr,Dm] = this.computeValues_(fr,fm,Kr,Km,Dr,Dm); 51 | % Go to the next force 52 | if ~isempty(this.next) 53 | [fr,fm,Kr,Km,Dr,Dm] = this.next.computeValues(fr,fm,Kr,Km,Dr,Dm); 54 | end 55 | end 56 | end 57 | 58 | %% 59 | function V = computeEnergy(this,V) 60 | if nargin == 1 61 | V = 0; 62 | end 63 | V = this.computeEnergy_(V); 64 | if ~isempty(this.next) 65 | V = this.next.computeEnergy(V); 66 | end 67 | end 68 | 69 | %% 70 | function draw(this) 71 | this.draw_(); 72 | if ~isempty(this.next) 73 | this.next.draw(); 74 | end 75 | end 76 | end 77 | 78 | %% 79 | methods (Access = protected) 80 | %% 81 | function init_(this) %#ok 82 | end 83 | 84 | %% 85 | function [fr,fm,Kr,Km,Dr,Dm] = computeValues_(this,fr,fm,Kr,Km,Dr,Dm) %#ok<*INUSL> 86 | end 87 | 88 | %% 89 | function V = computeEnergy_(this,V) 90 | end 91 | 92 | %% 93 | function draw_(this) %#ok 94 | end 95 | 96 | end 97 | end 98 | 99 | -------------------------------------------------------------------------------- /c++/PCG/cmake/FindJSONCPP.cmake: -------------------------------------------------------------------------------- 1 | # Find jsoncpp 2 | # 3 | # Find the jsoncpp includes and library 4 | # 5 | # if you need to add a custom library search path, do it via CMAKE_PREFIX_PATH 6 | # 7 | # This module defines 8 | # JSONCPP_INCLUDE_DIRS, where to find header, etc. 9 | # JSONCPP_LIBRARIES, the libraries needed to use jsoncpp. 10 | # JSONCPP_FOUND, If false, do not try to use jsoncpp. 11 | # JSONCPP_INCLUDE_PREFIX, include prefix for jsoncpp 12 | # References: 13 | # - https://github.com/majorx234/FindJsoncpp.cmake/blob/master/FindJsoncpp.cmake 14 | 15 | # Additional modules 16 | include(FindPackageHandleStandardArgs) 17 | 18 | IF(NOT JSONCPP_ROOT_DIR) 19 | SET(JSONCPP_ROOT_DIR "$ENV{JSONCPP_ROOT_DIR}") 20 | ENDIF() 21 | 22 | if (WIN32) 23 | # Find include files 24 | find_path( 25 | JSONCPP_INCLUDE_DIR 26 | NAMES json/json.h 27 | PATHS 28 | $ENV{PROGRAMFILES}/include/ 29 | ${JSONCPP_ROOT_DIR}/include/ 30 | DOC "The directory where jsoncpp/json/json.h or json/json.h resides") 31 | 32 | # Find library files 33 | find_library( 34 | JSONCPP_LIBRARY 35 | NAMES jsoncpp 36 | PATHS 37 | $ENV{PROGRAMFILES}/lib/ 38 | ${JSONCPP_ROOT_DIR}/lib/ 39 | ${JSONCPP_ROOT_DIR}/src/ 40 | ${JSONCPP_ROOT_DIR}/src/Release/ 41 | ${JSONCPP_ROOT_DIR}/src/Debug/) 42 | else() 43 | # Find include files 44 | find_path( 45 | JSONCPP_INCLUDE_DIR 46 | NAMES json/json.h 47 | PATHS 48 | ${JSONCPP_ROOT_DIR}/include/ 49 | DOC "The directory where jsoncpp/json/json.h or json/json.h resides") 50 | 51 | # Find library files 52 | # Try to use static libraries 53 | find_library( 54 | JSONCPP_LIBRARY 55 | NAMES jsoncpp libjsoncpp 56 | PATHS 57 | /usr/lib64/ 58 | /usr/lib/ 59 | /usr/local/lib64/ 60 | /usr/local/lib/ 61 | /sw/lib/ 62 | /opt/local/lib/ 63 | ${JSONCPP_ROOT_DIR}/lib/ 64 | ${JSONCPP_ROOT_DIR}/src/ 65 | DOC "The JSONCPP library") 66 | endif() 67 | 68 | # Handle REQUIRD argument, define *_FOUND variable 69 | find_package_handle_standard_args(jsoncpp DEFAULT_MSG JSONCPP_INCLUDE_DIR JSONCPP_LIBRARY) 70 | if (JSONCPP_FOUND) 71 | set(JSONCPP_INCLUDE_DIRS ${JSONCPP_INCLUDE_DIR}) 72 | set(JSONCPP_LIBRARIES ${JSONCPP_LIBRARY}) 73 | endif() 74 | 75 | # Hide some variables 76 | mark_as_advanced (JSONCPP_INCLUDE_DIR JSONCPP_LIBRARY) 77 | -------------------------------------------------------------------------------- /c++/PCG/src/JointPowered.cpp: -------------------------------------------------------------------------------- 1 | #include "JointPowered.h" 2 | 3 | #include "RigidBodyUtility.h" 4 | 5 | #include "Block.h" 6 | #include "LinkageSystem.h" 7 | #include "State.h" 8 | #include "StateDeriv.h" 9 | #include "StateSolve.h" 10 | 11 | void JointPowered::updateMaxSparse(std::unique_ptr& SS, const std::unique_ptr& LS, const std::unique_ptr& S, Matrix6d & parentAdj, Matrix6d & childAdj, Eigen::Vector3d & g) const 12 | { 13 | int numObj = (int)LS->blocks.size(); 14 | int row = SS->row; 15 | 16 | int parentIndex; 17 | int childIndex; 18 | if (!root) 19 | { 20 | parentIndex = LS->find(parentName)->joint->jindex; 21 | } 22 | childIndex = LS->find(childName)->joint->jindex; 23 | 24 | // Constrain translational movement 25 | for (int r = 0; r < 3; ++r) 26 | { 27 | for (int c = 0; c < 6; ++c) 28 | { 29 | if (!root) 30 | { 31 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, parentIndex * 6 + c, parentAdj(r + 3, c))); 32 | SS->LHSlist.push_back(Eigen::Triplet(parentIndex * 6 + c, numObj * 6 + row + r, parentAdj(r + 3, c))); 33 | } 34 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, childIndex * 6 + c, -1 * childAdj(r + 3, c))); 35 | SS->LHSlist.push_back(Eigen::Triplet(childIndex * 6 + c, numObj * 6 + row + r, -1 * childAdj(r + 3, c))); 36 | } 37 | // Load the corrective factor -1/h*g into f 38 | SS->fn[numObj * 6 + row + r] = g[r]; 39 | } 40 | row = row + 3; 41 | 42 | // Movement constraint 43 | for (int c = 0; c < 6; ++c) 44 | { 45 | if (!root) 46 | { 47 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row, parentIndex * 6 + c, parentAdj(2, c))); 48 | SS->LHSlist.push_back(Eigen::Triplet(parentIndex * 6 + c, numObj * 6 + row, parentAdj(2, c))); 49 | } 50 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row, childIndex * 6 + c, -1 * childAdj(2, c))); 51 | SS->LHSlist.push_back(Eigen::Triplet(childIndex * 6 + c, numObj * 6 + row, -1 * childAdj(2, c))); 52 | } 53 | // Load the rotation speed 54 | SS->fn[numObj * 6 + row] = cpower[0]; 55 | row = row + 1; 56 | 57 | SS->row = row; 58 | } 59 | 60 | void JointPowered::mapSelf(int c_i, std::unique_ptr& S) 61 | { 62 | } 63 | 64 | void JointPowered::updateSelf() 65 | { 66 | } 67 | -------------------------------------------------------------------------------- /matlab/+redmax/SpringPointDirection.m: -------------------------------------------------------------------------------- 1 | classdef SpringPointDirection < redmax.Spring 2 | %SpringPointDirection A linear force at a point in a particular direction 3 | 4 | %% 5 | properties 6 | stiffness 7 | body % Body to apply the wrench to 8 | x_i % Application point in local coords 9 | d_w % Force direction in world coords 10 | end 11 | 12 | methods 13 | function this = SpringPointDirection(body,x_i) 14 | this = this@redmax.Spring(); 15 | this.body = body; 16 | this.x_i = x_i; 17 | this.d_w = [0 0 0]'; 18 | this.stiffness = 0; 19 | end 20 | 21 | %% 22 | function setStiffness(this,stiffness) 23 | this.stiffness = stiffness; 24 | end 25 | 26 | %% 27 | function setDirection(this,d_w) 28 | this.d_w = d_w; 29 | end 30 | end 31 | 32 | %% 33 | methods (Access = protected) 34 | %% 35 | function [f,K,D] = computeForceStiffnessDamping_(this,f,K,D) 36 | [f_,K_] = this.computeFK(); 37 | idxM = this.body.idxM; 38 | f(idxM) = f(idxM) + f_; 39 | K(idxM,idxM) = K(idxM,idxM) + K_; 40 | % For recursive dynamics 41 | this.body.wext_i = this.body.wext_i + f_; 42 | this.body.Kmdiag = this.body.Kmdiag + K_; 43 | end 44 | 45 | %% 46 | function V = computeEnergy_(this,V) 47 | % TODO: this assumes that the point is the local origin 48 | %x_w = this.body.E_wi*[this.x_i;1]; % point in world coords 49 | %V = V - this.stiffness*x_w(1:3)'*this.d_w; 50 | end 51 | 52 | %% 53 | function y = computeStiffnessProd_(this,x,y) 54 | [~,K] = this.computeFK(); 55 | idxM = this.body.idxM; 56 | y(idxM) = y(idxM) + K*x(idxM); 57 | end 58 | 59 | %% 60 | function draw_(this) 61 | x_w = this.body.E_wi*[this.x_i;1]; 62 | xs = [x_w(1:3),x_w(1:3)+this.d_w]; 63 | plot3(xs(1,:),xs(2,:),xs(3,:),'r-'); 64 | end 65 | end 66 | 67 | %% 68 | methods (Access = private) 69 | %% 70 | function [f,K] = computeFK(this) 71 | f_w = this.stiffness*this.d_w; 72 | R = this.body.E_wi(1:3,1:3); 73 | G = se3.Gamma(this.x_i); 74 | f_i = R'*f_w; 75 | f = G'*f_i; 76 | % df = [ [xl]*[R'*fw] 0 ] 77 | % [ [R'*fw] 0 ] 78 | % Not symmetric! 79 | df = zeros(6); 80 | fibrac = se3.brac(f_i); 81 | df(1:3,1:3) = se3.brac(this.x_i)*fibrac; 82 | df(4:6,1:3) = fibrac; 83 | K = 0.5*(df + df'); % symmetrize 84 | K = 0*K; % DISABLE stiffness matrix 85 | end 86 | end 87 | end 88 | 89 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/TaskBDF1.m: -------------------------------------------------------------------------------- 1 | classdef (Abstract) TaskBDF1 < handle 2 | %TaskBDF1 A user-defined objective to be minimized 3 | 4 | %% 5 | properties 6 | scene 7 | p % parameters 8 | P % objective value 9 | wreg % regularizer weight 10 | dgdp % 11 | dPdq % 12 | end 13 | 14 | %% 15 | methods 16 | %% 17 | function this = TaskBDF1(scene,nparams) 18 | this.scene = scene; 19 | this.p = zeros(nparams,1); 20 | this.P = 0; 21 | this.wreg = 1; 22 | this.dgdp = []; 23 | this.dPdq = {}; 24 | end 25 | 26 | %% Initializes the task 27 | function init(this) 28 | nr = redmax.Scene.countR(); 29 | np = length(this.p); 30 | this.P = 0; 31 | nsteps = this.scene.nsteps; 32 | this.dPdq = cell(1,nsteps); 33 | this.dgdp = sparse(nsteps*nr,np); 34 | end 35 | 36 | %% Applies the parameters to the current step 37 | function applyStep(this) %#ok 38 | end 39 | 40 | %% Computes objective value and derivatives after a sim step 41 | function calcStep(this) %#ok 42 | end 43 | 44 | %% Computes objective value and derivatives after the sim 45 | function [P,dPdp] = calcFinal(this) 46 | nr = redmax.Scene.countR(); 47 | 48 | % Assume that all tasks have a simple regularizer 49 | P = this.P + this.wreg*0.5*(this.p'*this.p); 50 | 51 | % Compute derivative 52 | nsteps = this.scene.nsteps; 53 | z = zeros(nsteps*nr,1); 54 | h = this.scene.h; 55 | for k = this.scene.nsteps : -1 : 1 56 | yk = this.dPdq{k}; 57 | kk0 = (k-1)*nr + (1:nr); % indices for time step k 58 | kk1 = kk0 + nr; % indices for time step k+1 59 | kk2 = kk1 + nr; % indices for time step k+2 60 | if k < nsteps 61 | % Add contributions from step k+1 62 | M = this.scene.history(k+1).M; 63 | D = this.scene.history(k+1).D; 64 | block = -2*M + h*D; 65 | yk = yk - block'*z(kk1); 66 | end 67 | if k < nsteps - 1 68 | % Add contributions from step k+2 69 | M = this.scene.history(k+2).M; 70 | block = M; 71 | yk = yk - block'*z(kk2); 72 | end 73 | % Solve by the diagonal 74 | Hl = this.scene.history(k).Hl; 75 | Hu = this.scene.history(k).Hu; 76 | Hp = this.scene.history(k).Hp; 77 | zkk0(Hp) = Hl'\(Hu'\yk); 78 | z(kk0) = zkk0; 79 | end 80 | dPdp = this.wreg*this.p' - z'*this.dgdp; 81 | end 82 | 83 | %% Draws the current task state 84 | function draw(this) %#ok 85 | end 86 | end 87 | end 88 | -------------------------------------------------------------------------------- /matlab-simple/+redmax/JointRevolute.m: -------------------------------------------------------------------------------- 1 | classdef JointRevolute < redmax.Joint 2 | %% 3 | properties 4 | axis % Axis of rotation 5 | radius % Radius for contact 6 | height % Height for contact 7 | end 8 | 9 | %% 10 | methods 11 | %% 12 | function this = JointRevolute(parent,body,axis) 13 | this = this@redmax.Joint(parent,body,1); 14 | this.axis = axis; 15 | this.radius = 1.0; 16 | this.height = 1.0; 17 | end 18 | 19 | %% 20 | function setGeometry(this,radius,height) 21 | this.radius = radius; 22 | this.height = height; 23 | end 24 | end 25 | 26 | %% 27 | methods (Access = protected) 28 | %% 29 | function update_(this) 30 | this.Q = [se3.aaToMat(this.axis,this.q),[0 0 0]'; 0 0 0 1]; 31 | this.S(1:3) = this.axis; 32 | end 33 | 34 | %% 35 | function draw_(this) 36 | n = 8; 37 | E_ja = eye(4); 38 | z = [0 0 1]'; 39 | angle = acos(max(-1.0, min(this.axis'*z, 1.0))); 40 | E_ja(1:3,1:3) = se3.aaToMat(cross(this.axis,z),angle); 41 | E = this.E_wj*E_ja; 42 | % Cylinder 43 | r = this.radius; 44 | h = this.height; 45 | S = diag([1,1,h,1]); 46 | T = [eye(3),[0 0 -h/2]'; 0 0 0 1]; 47 | [X,Y,Z] = cylinder(r,n); 48 | X = reshape(X,1,2*(n+1)); 49 | Y = reshape(Y,1,2*(n+1)); 50 | Z = reshape(Z,1,2*(n+1)); 51 | XYZ = E*T*S*[X;Y;Z;ones(1,2*(n+1))]; 52 | X0 = reshape(XYZ(1,:),2,n+1); 53 | Y0 = reshape(XYZ(2,:),2,n+1); 54 | Z0 = reshape(XYZ(3,:),2,n+1); 55 | %surf(X,Y,Z,'FaceColor',this.body.color); 56 | % Ends 57 | % https://www.mathworks.com/matlabcentral/answers/320174-plot-a-surface-on-a-circle-and-annulus 58 | [T,R] = meshgrid(linspace(0,2*pi,n+1),linspace(0,r,2)); 59 | X = R.*cos(T); 60 | Y = R.*sin(T); 61 | Z = -h/2*ones(size(R)); 62 | X = reshape(X,1,2*(n+1)); 63 | Y = reshape(Y,1,2*(n+1)); 64 | Z = reshape(Z,1,2*(n+1)); 65 | XYZ = E*[X;Y;Z;ones(1,2*(n+1))]; 66 | X1 = reshape(XYZ(1,:),2,n+1); 67 | Y1 = reshape(XYZ(2,:),2,n+1); 68 | Z1 = reshape(XYZ(3,:),2,n+1); 69 | %surf(X,Y,Z,'FaceColor',this.body.color); 70 | X = R.*cos(T); 71 | Y = R.*sin(T); 72 | Z = h/2*ones(size(R)); 73 | X = reshape(X,1,2*(n+1)); 74 | Y = reshape(Y,1,2*(n+1)); 75 | Z = reshape(Z,1,2*(n+1)); 76 | XYZ = E*[X;Y;Z;ones(1,2*(n+1))]; 77 | X2 = reshape(XYZ(1,:),2,n+1); 78 | Y2 = reshape(XYZ(2,:),2,n+1); 79 | Z2 = reshape(XYZ(3,:),2,n+1); 80 | surf([X0 X1 X2],[Y0 Y1 Y2],[Z0 Z1 Z2],'FaceColor',this.body.color); 81 | end 82 | end 83 | end 84 | -------------------------------------------------------------------------------- /c++/PCG/cmake/FindGLFW.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # Find GLFW 3 | # 4 | # Try to find GLFW library. 5 | # This module defines the following variables: 6 | # - GLFW_INCLUDE_DIRS 7 | # - GLFW_LIBRARIES 8 | # - GLFW_FOUND 9 | # 10 | # The following variables can be set as arguments for the module. 11 | # - GLFW_ROOT_DIR : Root library directory of GLFW 12 | # - GLFW_USE_STATIC_LIBS : Specifies to use static version of GLFW library (Windows only) 13 | # 14 | # References: 15 | # - https://github.com/progschj/OpenGL-Examples/blob/master/cmake_modules/FindGLFW.cmake 16 | # - https://bitbucket.org/Ident8/cegui-mk2-opengl3-renderer/src/befd47200265/cmake/FindGLFW.cmake 17 | # - https://github.com/lighttransport/nanogi/blob/master/cmake/FindGLFW.cmake 18 | # - https://github.com/LevSmirnov1997/vse/blob/d11b7d30f4451e2c990d76fe88ef72599963fe94/al/cmake/FindGLFW.cmake 19 | 20 | # Additional modules 21 | include(FindPackageHandleStandardArgs) 22 | 23 | if (WIN32) 24 | # Find include files 25 | find_path( 26 | GLFW_INCLUDE_DIR 27 | NAMES GLFW/glfw3.h 28 | PATHS 29 | $ENV{PROGRAMFILES}/include/ 30 | ${GLFW_ROOT_DIR}/include/ 31 | DOC "The directory where GLFW/glfw.h resides") 32 | 33 | # Find library files 34 | find_library( 35 | GLFW_LIBRARY 36 | NAMES glfw3 37 | PATHS 38 | $ENV{PROGRAMFILES}/lib/ 39 | ${GLFW_ROOT_DIR}/lib/ 40 | ${GLFW_ROOT_DIR}/src/ 41 | ${GLFW_ROOT_DIR}/src/Release/ 42 | ${GLFW_ROOT_DIR}/src/Debug/) 43 | 44 | else() 45 | # Find include files 46 | find_path( 47 | GLFW_INCLUDE_DIR 48 | NAMES GLFW/glfw3.h 49 | PATHS 50 | /usr/include/ 51 | /usr/local/include/ 52 | /sw/include/ 53 | /opt/local/include/ 54 | ${GLFW_ROOT_DIR}/include/ 55 | DOC "The directory where GL/glfw.h resides") 56 | 57 | # Find library files 58 | # Try to use static libraries 59 | find_library( 60 | GLFW_LIBRARY 61 | NAMES glfw3 glfw libglfw.so 62 | PATHS 63 | /usr/lib64/ 64 | /usr/lib/ 65 | /usr/local/lib64/ 66 | /usr/local/lib/ 67 | /sw/lib/ 68 | /opt/local/lib/ 69 | ${GLFW_ROOT_DIR}/lib/ 70 | ${GLFW_ROOT_DIR}/src/ 71 | DOC "The GLFW library") 72 | endif() 73 | 74 | # Handle REQUIRD argument, define *_FOUND variable 75 | find_package_handle_standard_args(GLFW DEFAULT_MSG GLFW_INCLUDE_DIR GLFW_LIBRARY) 76 | 77 | # Define GLFW_LIBRARIES and GLFW_INCLUDE_DIRS 78 | if (GLFW_FOUND) 79 | set(GLFW_LIBRARIES ${OPENGL_LIBRARIES} ${GLFW_LIBRARY}) 80 | set(GLFW_INCLUDE_DIRS ${GLFW_INCLUDE_DIR}) 81 | endif() 82 | 83 | # Hide some variables 84 | mark_as_advanced(GLFW_INCLUDE_DIR GLFW_LIBRARY) -------------------------------------------------------------------------------- /c++/PCG/src/online/MatrixStack.cpp: -------------------------------------------------------------------------------- 1 | #include "MatrixStack.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #define GLM_FORCE_RADIANS 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | 13 | MatrixStack::MatrixStack() 14 | { 15 | mstack = make_shared< stack >(); 16 | mstack->push(glm::mat4(1.0)); 17 | } 18 | 19 | MatrixStack::~MatrixStack() 20 | { 21 | } 22 | 23 | void MatrixStack::pushMatrix() 24 | { 25 | const glm::mat4 &top = mstack->top(); 26 | mstack->push(top); 27 | assert(mstack->size() < 100); 28 | } 29 | 30 | void MatrixStack::popMatrix() 31 | { 32 | assert(!mstack->empty()); 33 | mstack->pop(); 34 | // There should always be one matrix left. 35 | assert(!mstack->empty()); 36 | } 37 | 38 | void MatrixStack::loadIdentity() 39 | { 40 | glm::mat4 &top = mstack->top(); 41 | top = glm::mat4(1.0); 42 | } 43 | 44 | void MatrixStack::translate(const glm::vec3 &t) 45 | { 46 | glm::mat4 &top = mstack->top(); 47 | top *= glm::translate(t); 48 | } 49 | 50 | void MatrixStack::translate(float x, float y, float z) 51 | { 52 | translate(glm::vec3(x, y, z)); 53 | } 54 | 55 | void MatrixStack::scale(const glm::vec3 &s) 56 | { 57 | glm::mat4 &top = mstack->top(); 58 | top *= glm::scale(s); 59 | } 60 | 61 | void MatrixStack::scale(float x, float y, float z) 62 | { 63 | scale(glm::vec3(x, y, z)); 64 | } 65 | 66 | void MatrixStack::scale(float s) 67 | { 68 | scale(glm::vec3(s, s, s)); 69 | } 70 | 71 | void MatrixStack::rotate(float angle, const glm::vec3 &axis) 72 | { 73 | glm::mat4 &top = mstack->top(); 74 | top *= glm::rotate(angle, axis); 75 | } 76 | 77 | void MatrixStack::rotate(float angle, float x, float y, float z) 78 | { 79 | rotate(angle, glm::vec3(x, y, z)); 80 | } 81 | 82 | void MatrixStack::multMatrix(const glm::mat4 &matrix) 83 | { 84 | glm::mat4 &top = mstack->top(); 85 | top *= matrix; 86 | } 87 | 88 | const glm::mat4 &MatrixStack::topMatrix() const 89 | { 90 | return mstack->top(); 91 | } 92 | 93 | void MatrixStack::print(const glm::mat4 &mat, const char *name) 94 | { 95 | if(name) { 96 | printf("%s = [\n", name); 97 | } 98 | for(int i = 0; i < 4; ++i) { 99 | for(int j = 0; j < 4; ++j) { 100 | // mat[j] returns the jth column 101 | printf("%- 5.2f ", mat[j][i]); 102 | } 103 | printf("\n"); 104 | } 105 | if(name) { 106 | printf("];"); 107 | } 108 | printf("\n"); 109 | } 110 | 111 | void MatrixStack::print(const char *name) const 112 | { 113 | print(mstack->top(), name); 114 | } 115 | -------------------------------------------------------------------------------- /matlab/+redmax/Spring.m: -------------------------------------------------------------------------------- 1 | classdef Spring < handle 2 | %Spring Mass-less spring acting on bodies 3 | 4 | %% 5 | properties 6 | next % Next spring in traversal order 7 | end 8 | 9 | %% 10 | methods 11 | %% 12 | function this = Spring() 13 | end 14 | 15 | %% 16 | function [f,K,D] = computeForceStiffnessDamping(this,f,K,D) 17 | nm = redmax.Scene.countM(); 18 | if nargin == 1 19 | f = zeros(nm,1); 20 | K = zeros(nm); 21 | D = zeros(nm); 22 | elseif nargin == 2 23 | K = zeros(nm); 24 | D = zeros(nm); 25 | end 26 | if isempty(K) 27 | K = zeros(nm); 28 | end 29 | [f,K,D] = this.computeForceStiffnessDamping_(f,K,D); 30 | if ~isempty(this.next) 31 | [f,K,D] = this.next.computeForceStiffnessDamping(f,K,D); 32 | end 33 | end 34 | 35 | %% 36 | function V = computeEnergy(this,V) 37 | V = this.computeEnergy_(V); 38 | if ~isempty(this.next) 39 | V = this.next.computeEnergy(V); 40 | end 41 | end 42 | 43 | %% 44 | function y = computeStiffnessProd(this,x,y) 45 | % Computes y = K*x 46 | if nargin == 2 47 | nm = redmax.Scene.countM(); 48 | y = zeros(nm,1); 49 | end 50 | y = this.computeStiffnessProd_(x,y); 51 | if ~isempty(this.next) 52 | y = this.next.computeStiffnessProd(x,y); 53 | end 54 | end 55 | 56 | %% 57 | function y = computeDampingProd(this,x,y) 58 | % Computes y = D*x 59 | if nargin == 2 60 | nm = redmax.Scene.countM(); 61 | y = zeros(nm,1); 62 | end 63 | y = this.computeDampingProd_(x,y); 64 | if ~isempty(this.next) 65 | y = this.next.computeDampingProd(x,y); 66 | end 67 | end 68 | 69 | %% 70 | function u = getStiffnessRank1(this) 71 | nm = redmax.Scene.countM(); 72 | u = zeros(nm,1); 73 | u = this.getStiffnessRank1_(u); 74 | end 75 | 76 | %% 77 | function draw(this) 78 | this.draw_(); 79 | if ~isempty(this.next) 80 | this.next.draw(); 81 | end 82 | end 83 | end 84 | 85 | %% 86 | methods (Access = protected) 87 | %% 88 | function [f,K,D] = computeForceStiffnessDamping_(this,f,K,D) %#ok<*INUSL> 89 | end 90 | 91 | %% 92 | function V = computeEnergy_(this,V) 93 | end 94 | 95 | %% 96 | function y = computeStiffnessProd_(this,x,y) 97 | end 98 | 99 | %% 100 | function y = computeDampingProd_(this,x,y) 101 | end 102 | 103 | %% 104 | function u = getStiffnessRank1_(this,u) 105 | end 106 | 107 | %% 108 | function draw_(this) %#ok 109 | end 110 | 111 | end 112 | end 113 | 114 | -------------------------------------------------------------------------------- /matlab-simple/testRigid.m: -------------------------------------------------------------------------------- 1 | %% 2 | function testRigid 3 | 4 | h = 5e-3; 5 | tEnd = 2; 6 | drawHz = 60; 7 | grav = [0 0 -1]' * 98; 8 | rhoV = 1e0; % volume density for rigid bodies 9 | 10 | rigids = []; 11 | rigids(end+1).whd = [10 1 1]'; 12 | rigids(end).E = eye(4); 13 | rigids(end).phi = [0 3 0 0 0 90]'; 14 | 15 | % Rigid body mass 16 | nrbs = length(rigids); 17 | for i = 1 : nrbs 18 | % The rigid frame is attached to the center of mass of the body, with 19 | % the axes aligned to the inertial frame. Also, we use velocities in 20 | % body space, so that the mass matrix becomes diagonal. 21 | whd = rigids(i).whd; 22 | mass = rhoV * prod(whd); 23 | rigids(i).m(1) = (1/12) * mass * whd([2,3])' * whd([2,3]); 24 | rigids(i).m(2) = (1/12) * mass * whd([3,1])' * whd([3,1]); 25 | rigids(i).m(3) = (1/12) * mass * whd([1,2])' * whd([1,2]); 26 | rigids(i).m(4) = mass; 27 | rigids(i).m(5) = mass; 28 | rigids(i).m(6) = mass; 29 | fprintf('rigid%d: %.2f g\n', i, mass); 30 | end 31 | 32 | % System matrix indices 33 | n = 0; 34 | for i = 1 : nrbs 35 | % Since we're using maximal coordinates, each rigid body gets 6 degrees 36 | % of freedom 37 | rigids(i).i = n + (1:6); %#ok<*AGROW> 38 | n = n + 6; 39 | end 40 | 41 | % Run 42 | t0 = -inf; 43 | for t = 0 : h : tEnd 44 | % Draw scene 45 | if t - t0 > 1 / drawHz 46 | drawTest(t,rigids); 47 | t0 = t; 48 | end 49 | 50 | % Fill M, f, v 51 | M = zeros(n); 52 | f = zeros(n,1); 53 | v = zeros(n,1); 54 | for i = 1 : nrbs 55 | rb = rigids(i); 56 | R = rb.E(1:3,1:3); 57 | ir = rb.i; 58 | phi = rb.phi; 59 | v(ir) = phi; 60 | Mr = diag(rb.m); 61 | M(ir,ir) = Mr; % mass matrix 62 | fcor = se3.ad(phi)' * Mr * phi; % coriolis force 63 | fext = [zeros(3,1); rb.m(4) * R' * grav]; % gravity 64 | f(ir) = fcor + fext; 65 | end 66 | 67 | % Update velocities and positions 68 | v = M\(M*v + h*f); 69 | for i = 1 : nrbs 70 | rigids(i).phi = v(rigids(i).i); 71 | rigids(i).E = rigids(i).E*se3.exp(h*rigids(i).phi); 72 | end 73 | 74 | end 75 | 76 | end 77 | 78 | %% 79 | function drawTest(t,rigids) 80 | 81 | if t == 0 82 | clf; 83 | xlabel('X'); 84 | ylabel('Y'); 85 | zlabel('Z'); 86 | axis equal; 87 | axis(20*[-1 1 -1 1 -0 2]); 88 | grid on; 89 | view([0,0]); 90 | end 91 | cla; 92 | hold on; 93 | 94 | % Draw rigid bodies 95 | for i = 1 : length(rigids) 96 | se3.drawCuboid(rigids(i).E,rigids(i).whd); 97 | end 98 | 99 | str = sprintf('t = %.4f', t); 100 | title(str); 101 | drawnow; 102 | 103 | end 104 | -------------------------------------------------------------------------------- /c++/PCG/src/JointFixed.cpp: -------------------------------------------------------------------------------- 1 | #include "JointFixed.h" 2 | 3 | #include "Block.h" 4 | #include "LinkageSystem.h" 5 | #include "State.h" 6 | #include "StateDeriv.h" 7 | #include "StateSolve.h" 8 | 9 | JointFixed::JointFixed(std::unique_ptr& S, std::string pn, std::string cn, Eigen::Vector3d pp, Eigen::Vector3d cp, bool r, int i, Eigen::Matrix3d Q0) : 10 | Joint(S, JType::Fixed, pn, cn, pp, cp, r, i, 0) 11 | { 12 | this->S = Eigen::MatrixXd::Zero(6, 0); 13 | this->ST = Eigen::MatrixXd::Zero(0, 6); 14 | Sdot = Eigen::MatrixXd::Zero(6, 0); 15 | 16 | q0 = Eigen::VectorXd(constraintNum); 17 | Q = Eigen::Matrix4d::Identity(); 18 | Q.block<3,3>(0,0) = Q0; 19 | 20 | Pr = Eigen::MatrixXd::Zero(0, 0); 21 | Psi = Eigen::MatrixXd::Zero(0, 0); 22 | Kmd = Matrix6d::Zero(); 23 | Dmd = Matrix6d::Zero(); 24 | constraint_index = -1; 25 | } 26 | 27 | void JointFixed::updateMaxSparse(std::unique_ptr& SS, const std::unique_ptr& LS, const std::unique_ptr& S, Matrix6d & parentAdj, Matrix6d & childAdj, Eigen::Vector3d & g) const 28 | { 29 | int numObj = (int)LS->blocks.size(); 30 | int row = SS->row; 31 | 32 | int parentIndex; 33 | int childIndex; 34 | if (!root) 35 | { 36 | parentIndex = LS->find(parentName)->joint->jindex; 37 | } 38 | childIndex = LS->find(childName)->joint->jindex; 39 | 40 | // Constrain translational movement 41 | for (int r = 0; r < 6; ++r) 42 | { 43 | for (int c = 0; c < 6; ++c) 44 | { 45 | if (!root) 46 | { 47 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, parentIndex * 6 + c, parentAdj(r + 3, c))); 48 | SS->LHSlist.push_back(Eigen::Triplet(parentIndex * 6 + c, numObj * 6 + row + r, parentAdj(r + 3, c))); 49 | } 50 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, childIndex * 6 + c, -1 * childAdj(r + 3, c))); 51 | SS->LHSlist.push_back(Eigen::Triplet(childIndex * 6 + c, numObj * 6 + row + r, -1 * childAdj(r + 3, c))); 52 | } 53 | // Load the corrective factor -1/h*g into f 54 | SS->fn[numObj * 6 + row + r] = g[r]; 55 | } 56 | row = row + 6; 57 | 58 | SS->row = row; 59 | } 60 | 61 | void JointFixed::mapSelf(int c_i, std::unique_ptr& S) 62 | { 63 | //new (&q) Eigen::Map(&(S->q[c_i]), constraintNum); 64 | q = Eigen::VectorXd::Zero(constraintNum); 65 | 66 | //new (&qdot) Eigen::Map(&(S->qdot[c_i]), constraintNum); 67 | qdot = Eigen::VectorXd::Zero(constraintNum); 68 | 69 | updateSelf(); 70 | } 71 | 72 | void JointFixed::updateSelf() 73 | { 74 | // Q does not change 75 | } 76 | 77 | Vector6d JointFixed::Sqdot(const std::unique_ptr& S) 78 | { 79 | Vector6d temp = Vector6d::Zero(); 80 | return temp; 81 | } 82 | -------------------------------------------------------------------------------- /c++/PCG/src/JointSpringy.cpp: -------------------------------------------------------------------------------- 1 | #include "JointSpringy.h" 2 | 3 | #include "RigidBodyUtility.h" 4 | 5 | #include "Block.h" 6 | #include "LinkageSystem.h" 7 | #include "State.h" 8 | #include "StateDeriv.h" 9 | #include "StateSolve.h" 10 | 11 | void JointSpringy::updateMaxSparse(std::unique_ptr& SS, const std::unique_ptr& LS, const std::unique_ptr& S, Matrix6d & parentAdj, Matrix6d & childAdj, Eigen::Vector3d & g) const 12 | { 13 | int numObj = (int)LS->blocks.size(); 14 | int row = SS->row; 15 | 16 | int parentIndex; 17 | int childIndex; 18 | if (!root) 19 | { 20 | parentIndex = LS->find(parentName)->joint->jindex; 21 | } 22 | childIndex = LS->find(childName)->joint->jindex; 23 | 24 | // Constrain translational movement 25 | for (int r = 0; r < 3; ++r) 26 | { 27 | for (int c = 0; c < 6; ++c) 28 | { 29 | if (!root) 30 | { 31 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, parentIndex * 6 + c, parentAdj(r + 3, c))); 32 | SS->LHSlist.push_back(Eigen::Triplet(parentIndex * 6 + c, numObj * 6 + row + r, parentAdj(r + 3, c))); 33 | } 34 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, childIndex * 6 + c, -1 * childAdj(r + 3, c))); 35 | SS->LHSlist.push_back(Eigen::Triplet(childIndex * 6 + c, numObj * 6 + row + r, -1 * childAdj(r + 3, c))); 36 | } 37 | // Load the corrective factor -1/h*g into f 38 | SS->fn[numObj * 6 + row + r] = g[r]; 39 | } 40 | row = row + 3; 41 | 42 | ///**************************************************************************************************************************** 43 | // TODO: acute rest angles, negative rest angles, DO NOT ALLOW THE JOINT TO WRAP ITSELF (360 degree rotations) 44 | // TODO: springy joints IF THEY AFFECT THE ROOT JOINT 45 | Eigen::Matrix4d bToJ0 = E_jp0 * Rigid::inverse(S->E[parentIndex]) * S->E[childIndex]; 46 | double denom = ((bToJ0.block<3, 3>(0, 0) * childPos).norm() * (E_jp0.block<3, 3>(0, 0) * parentPos).norm()); 47 | Eigen::Vector3d sinthetaN = Rigid::bracket3(bToJ0.block<3, 3>(0, 0) * childPos) * (E_jp0.block<3, 3>(0, 0) * parentPos) / 48 | ((bToJ0.block<3, 3>(0, 0) * childPos).norm() * (E_jp0.block<3, 3>(0, 0) * parentPos).norm()); 49 | double theta = asin(sinthetaN[2]) - (RB_M_PI - angle); 50 | SS->fn.segment<3>(parentIndex * 6) += E_jp0.inverse().block<3, 3>(0, 0) * Eigen::Vector3d::UnitZ() * k * theta; 51 | SS->fn.segment<3>(childIndex * 6) += -1 * S->E[childIndex].inverse().block<3, 3>(0, 0) * S->E[parentIndex].block<3, 3>(0, 0) * Eigen::Vector3d::UnitZ() * k *theta; 52 | 53 | SS->row = row; 54 | } 55 | 56 | void JointSpringy::mapSelf(int c_i, std::unique_ptr& S) 57 | { 58 | } 59 | 60 | void JointSpringy::updateSelf() 61 | { 62 | } 63 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # REDMAX: Efficient & Flexible Approach for Articulated Dynamics 2 | 3 | **NEW**: We now have an _analytically differentiable_ version! See the the [notes](notes.pdf) and the [reference MATLAB implementation](matlab-diff). 4 | 5 | Redmax was also used for [differentiable hand simulation](http://diffhand.csail.mit.edu), which was presented at RSS 2021. The associated [github repository](https://github.com/eanswer/DiffHand) contains the C++ implementation of redmax with Python bindings. 6 | 7 | 8 | ### Contents 9 | 10 | - `notes.pdf`: An extensive writeup with details on: 11 | - Maximal coordinates 12 | - Reduced coordinates 13 | - Analytical derivatives 14 | - Implicit integration 15 | - Adjoint method 16 | - `matlab-diff`: Object-oriented MATLAB implementation of differentiable redmax 17 | - Fully implicit time integration: BDF1 and BDF2 18 | - Parameter optimization with the adjoint method 19 | - Frictional contact with the ground [[Geilinger et al. 2020]](https://arxiv.org/pdf/2007.00987.pdf) 20 | - `matlab-simple`: Simpler object-oriented MATLAB implementation for getting started 21 | - `matlab`: Object-oriented MATLAB implementation with many features, including: 22 | - Recursive hybrid dynamics (Featherstone's algorithm) for comparison 23 | - Time integration using `ode45` or `euler` 24 | - Frictional dynamics with Bilateral Staggered Projections 25 | - Spline curve and surface joints [[Lee and Terzopoulos 2008]](http://web.cs.ucla.edu/~dt//papers/siggraph08/siggraph08.pdf) 26 | - `c++`: C++ implementation including Projected Block Jacobi Preconditioner 27 | 28 | 29 | ### Citation 30 | 31 | **ACM Transactions on Graphics, 38 (4) 104:1-104:10 (SIGGRAPH), 2019.** 32 | 33 | [Ying Wang](http://www.yingwang.io/), [Nicholas J. Weidner](http://weidnern.github.io/), [Margaret A. Baxter](https://www.linkedin.com/in/baxter-margareta/), [Yura Hwang](http://yurahwang.com/), [Danny M. Kaufman](http://dannykaufman.io), [Shinjiro Sueda](http://faculty.cs.tamu.edu/sueda/) 34 | 35 | @article{Wang2019, 36 | author = {Wang, Ying and Weidner, Nicholas J. and Baxter, Margaret A. and Hwang, Yura and Kaufman, Danny M. and Sueda, Shinjiro}, 37 | title = {\textsc{RedMax}: Efficient \& Flexible Approach for Articulated Dynamics}, 38 | year = {2019}, 39 | issue_date = {July 2019}, 40 | publisher = {ACM}, 41 | address = {New York, NY, USA}, 42 | volume = {38}, 43 | number = {4}, 44 | issn = {0730-0301}, 45 | url = {https://doi.org/10.1145/3306346.3322952}, 46 | doi = {10.1145/3306346.3322952}, 47 | journal = {{ACM} Trans.\ Graph.}, 48 | month = jul, 49 | articleno = {104}, 50 | numpages = {10}, 51 | keywords = {friction, rigid body dynamics, physical simulation, constraints, contact} 52 | } 53 | 54 | -------------------------------------------------------------------------------- /c++/PCG/src/JointHinge.cpp: -------------------------------------------------------------------------------- 1 | #include "JointHinge.h" 2 | 3 | #include "Block.h" 4 | #include "LinkageSystem.h" 5 | #include "State.h" 6 | #include "StateDeriv.h" 7 | #include "StateSolve.h" 8 | 9 | JointHinge::JointHinge(std::unique_ptr& S, std::string pn, std::string cn, Eigen::Vector3d pp, Eigen::Vector3d cp, bool r, int i, double angle, Eigen::Vector3d & axis, double k, double d) : 10 | Joint(S, JType::Hinge, pn, cn, pp, cp, r, i, 1, k, d) 11 | { 12 | this->axis = axis; 13 | 14 | this->S = Eigen::MatrixXd::Zero(6, 1); 15 | this->S.block<3, 1>(0, 0) = axis; 16 | this->ST = this->S.transpose(); 17 | Sdot = Eigen::MatrixXd::Zero(6, 1); 18 | 19 | q0 = Eigen::VectorXd(constraintNum); 20 | q0[0] = angle; 21 | 22 | Q = Eigen::Matrix4d::Identity(); 23 | 24 | Pr = Eigen::MatrixXd::Zero(1, 1); 25 | Psi = Eigen::MatrixXd::Zero(1, 1); 26 | Kmd = Matrix6d::Zero(); 27 | Dmd = Matrix6d::Zero(); 28 | } 29 | 30 | void JointHinge::updateMaxSparse(std::unique_ptr& SS, const std::unique_ptr& LS, const std::unique_ptr& S, Matrix6d & parentAdj, Matrix6d & childAdj, Eigen::Vector3d & g) const 31 | { 32 | int numObj = (int)LS->blocks.size(); 33 | int row = SS->row; 34 | 35 | int parentIndex; 36 | int childIndex; 37 | if (!root) 38 | { 39 | parentIndex = LS->find(parentName)->joint->jindex; 40 | } 41 | childIndex = LS->find(childName)->joint->jindex; 42 | 43 | // Constrain translational movement 44 | for (int r = 0; r < 3; ++r) 45 | { 46 | for (int c = 0; c < 6; ++c) 47 | { 48 | if (!root) 49 | { 50 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, parentIndex * 6 + c, parentAdj(r + 3, c))); 51 | SS->LHSlist.push_back(Eigen::Triplet(parentIndex * 6 + c, numObj * 6 + row + r, parentAdj(r + 3, c))); 52 | } 53 | SS->LHSlist.push_back(Eigen::Triplet(numObj * 6 + row + r, childIndex * 6 + c, -1 * childAdj(r + 3, c))); 54 | SS->LHSlist.push_back(Eigen::Triplet(childIndex * 6 + c, numObj * 6 + row + r, -1 * childAdj(r + 3, c))); 55 | } 56 | // Load the corrective factor -1/h*g into f 57 | SS->fn[numObj * 6 + row + r] = g[r]; 58 | } 59 | row = row + 3; 60 | 61 | SS->row = row; 62 | } 63 | 64 | void JointHinge::mapSelf(int c_i, std::unique_ptr& S) 65 | { 66 | new (&q) Eigen::Map(&(S->q[c_i]), constraintNum); 67 | q[0] = q0[0]; 68 | 69 | new (&qdot) Eigen::Map(&(S->qdot[c_i]), constraintNum); 70 | qdot = Eigen::VectorXd::Zero(constraintNum); 71 | 72 | updateSelf(); 73 | } 74 | 75 | void JointHinge::updateSelf() 76 | { 77 | Eigen::Matrix3d R_wj0; 78 | if (q.size() != 0) 79 | R_wj0 = Eigen::AngleAxisd(q[0], axis); 80 | else 81 | R_wj0 = Eigen::AngleAxisd(q0[0], axis); 82 | Q.block<3, 3>(0, 0) = R_wj0; 83 | } 84 | 85 | Vector6d JointHinge::Sqdot(const std::unique_ptr& S) 86 | { 87 | return this->S * qdot; 88 | } 89 | -------------------------------------------------------------------------------- /matlab/+redmax/BodyComposite.m: -------------------------------------------------------------------------------- 1 | classdef BodyComposite < redmax.Body 2 | %BodyComposite A body made of multiple bodies 3 | 4 | %% 5 | properties 6 | bodies 7 | Es % Transforms of the bodies 8 | I_c % Composite inertia 9 | end 10 | 11 | %% 12 | methods 13 | %% 14 | function this = BodyComposite(density) 15 | this = this@redmax.Body(density); 16 | this.bodies = {}; 17 | this.Es = {}; 18 | end 19 | 20 | %% 21 | function addBody(this,body,E) 22 | % E is the transform of body wrt parent joint. 23 | this.bodies{end+1} = body; 24 | this.Es{end+1} = E; 25 | end 26 | 27 | %% 28 | function E_jc = computeInertiaFrame(this) 29 | % Computes the composite inertia and frame. 30 | % The computations will be performed wrt the parent joint's 31 | % frame. We can't use world coords because we don't know where 32 | % these bodies are going to go. 33 | n = length(this.bodies); 34 | % Total mass 35 | mtotal = 0; 36 | for k = 1 : n 37 | body = this.bodies{k}; 38 | body.computeInertia_(); 39 | mtotal = mtotal + body.I_i(6); 40 | end 41 | % Center of mass 42 | xc = zeros(3,1); 43 | for k = 1 : n 44 | body = this.bodies{k}; 45 | E = this.Es{k}; 46 | xc = xc + (body.I_i(6)/mtotal)*E(1:3,4); 47 | end 48 | % Construct a frame at xc 49 | E_jc = eye(4); 50 | E_jc(1:3,4) = xc; 51 | % Sum the inertias at E_wc 52 | this.I_c = zeros(6); 53 | for k = 1 : n 54 | body = this.bodies{k}; 55 | E_jk = this.Es{k}; 56 | E_kj = se3.inv(E_jk); 57 | E_kc = E_kj * E_jc; 58 | Ad_kc = se3.Ad(E_kc); 59 | this.I_c = this.I_c + Ad_kc'*diag(body.I_i)*Ad_kc; 60 | end 61 | % Compute the rotated frame 62 | J = this.I_c(1:3,1:3); 63 | [V,D] = eig(J); 64 | if cross(V(:,1),V(:,2))'*V(:,3) < 0 65 | % Switch to right-handed coord frame 66 | V(1:3,3) = -V(1:3,3); 67 | end 68 | E_jc(1:3,1:3) = V; 69 | this.I_c = [diag(D)' mtotal mtotal mtotal]'; 70 | % Transform Es to be the transform of each body wrt the 71 | % composite inertia 72 | E_cj = se3.inv(E_jc); 73 | for k = 1 : n 74 | E_jb = this.Es{k}; 75 | E_cb = E_cj * E_jb; 76 | this.Es{k} = E_cb; 77 | end 78 | end 79 | 80 | %% 81 | function computeInertia_(this) 82 | % Computes inertia at body 83 | this.I_i = this.I_c; 84 | end 85 | 86 | %% 87 | function draw_(this) 88 | se3.drawAxis(this.E_wi,5); 89 | for k = 1 : length(this.bodies) 90 | body = this.bodies{k}; 91 | body.E_wi = this.E_wi * this.Es{k}; 92 | body.draw_(); 93 | end 94 | end 95 | 96 | %% 97 | function s = getAxisSize(this) 98 | s = 0; 99 | for k = 1 : length(this.bodies) 100 | body = this.bodies{k}; 101 | s = max(s,body.getAxisSize()); 102 | end 103 | end 104 | end 105 | end 106 | -------------------------------------------------------------------------------- /c++/PCG/src/Solver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #ifndef _Solver_ 3 | #define _Solver_ 4 | 5 | #define EIGEN_DONT_ALIGN_STATICALLY 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include "State.h" 13 | 14 | struct LinkageSystem; 15 | //struct State; 16 | struct StateDeriv; 17 | struct StateSolve; 18 | 19 | struct SolverDataTracker 20 | { 21 | double time_in_solve; 22 | int num_iterations; 23 | }; 24 | 25 | class Solver 26 | { 27 | private: 28 | void loadMSparse(std::unique_ptr &SS, 29 | std::unique_ptr &LS, 30 | std::unique_ptr &S); 31 | 32 | void computeRHS(std::unique_ptr &SS, 33 | const std::unique_ptr &LS, 34 | const std::unique_ptr &S); 35 | 36 | void pcdSaad2003(Eigen::VectorXd &result, 37 | const Eigen::VectorXd &LHSx0, 38 | const Eigen::VectorXd &b, 39 | std::unique_ptr &SS, 40 | std::unique_ptr &LS, 41 | std::unique_ptr &S, 42 | std::shared_ptr lmt = nullptr, 43 | double tol = 1e-6, int maxit = 1000); 44 | 45 | void pcdSaad2003_unopt(Eigen::VectorXd &result, 46 | const Eigen::VectorXd &LHSx0, 47 | const Eigen::VectorXd &b, 48 | std::unique_ptr &SS, 49 | std::unique_ptr &LS, 50 | std::unique_ptr &S, 51 | std::shared_ptr lmt = nullptr, 52 | double tol = 1e-6, int maxit = 1000); 53 | 54 | #ifdef REDMAX_PARDISO 55 | Eigen::VectorXd solvePardiso(std::unique_ptr &SS, 56 | std::unique_ptr &LS, 57 | std::unique_ptr &S); 58 | #endif 59 | 60 | Eigen::VectorXd solvePCG_unopt(std::unique_ptr &SS, 61 | std::unique_ptr &LS, 62 | std::unique_ptr &S); 63 | 64 | Eigen::VectorXd solvePCG(std::unique_ptr &SS, 65 | std::unique_ptr &LS, 66 | std::unique_ptr &S); 67 | 68 | public: 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | 71 | SolverDataTracker trackLastTimestep; 72 | 73 | Solver(); 74 | 75 | Eigen::VectorXd solve(std::unique_ptr &SS, 76 | std::unique_ptr &LS, 77 | std::unique_ptr &S); 78 | 79 | void printDenseToFile(Eigen::MatrixXd & MG, 80 | Eigen::VectorXd & f, 81 | Eigen::VectorXd & res, 82 | std::string id); 83 | 84 | void printSparseToFile(std::vector>& MG, 85 | Eigen::VectorXd & f, 86 | Eigen::VectorXd & res); 87 | 88 | void addSparseToFile(std::ofstream &outfile, std::string name, Eigen::MatrixXd &M); 89 | void addSparseToFile(std::ofstream &outfile, std::string name, Eigen::SparseMatrix &M); 90 | void addVectorToFile(std::ofstream &outfile, std::string name, Eigen::VectorXd &M); 91 | }; 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /matlab-simple/+redmax/Scene.m: -------------------------------------------------------------------------------- 1 | classdef Scene < handle 2 | %Scene Test scenes for redmax 3 | 4 | properties 5 | name 6 | bodies 7 | joints 8 | tspan 9 | hEuler 10 | drawHz 11 | grav 12 | waxis 13 | end 14 | 15 | methods 16 | function this = Scene() 17 | % Default values 18 | redmax.Scene.clear(); 19 | this.name = ''; 20 | this.bodies = {}; 21 | this.joints = {}; 22 | this.tspan = [0 2]; 23 | this.hEuler = 1e-2; 24 | this.drawHz = 10; 25 | this.grav = [0 0 -980]'; 26 | this.waxis = []; 27 | end 28 | 29 | function init(this) 30 | if usejava('jvm') 31 | colormap('default'); % Restore colormap mode 32 | end 33 | njoints = length(this.joints); 34 | joint0 = this.joints{1}; % Assumes joints{1} is root 35 | order = joint0.getTraversalOrder(); 36 | this.joints = this.joints(order); % Reorders joints 37 | for i = njoints : -1 : 1 38 | % Leaf-to-root ordering is better for reducing matrix fill 39 | this.joints{i}.countDofs(); % This will also call Body.countDofs() 40 | end 41 | for i = 1 : njoints 42 | if i < njoints 43 | this.joints{i}.next = this.joints{i+1}; 44 | end 45 | if i > 1 46 | this.joints{i}.prev = this.joints{i-1}; 47 | end 48 | end 49 | joint0.update(); 50 | nbodies = length(this.bodies); 51 | this.bodies = this.bodies(order); % same ordering as joints 52 | for i = 1 : nbodies 53 | this.bodies{i}.computeInertia(); 54 | if i < nbodies 55 | this.bodies{i}.next = this.bodies{i+1}; %#ok<*SAGROW> 56 | end 57 | end 58 | end 59 | 60 | %% 61 | function draw(this,t) 62 | if t == 0 63 | clf; 64 | xlabel('X'); 65 | ylabel('Y'); 66 | zlabel('Z'); 67 | axis equal; 68 | if ~isempty(this.waxis) 69 | axis(this.waxis); 70 | end 71 | ax = gca; 72 | ax.Clipping = 'off'; 73 | grid on; 74 | view(3); 75 | end 76 | cla; 77 | hold on; 78 | this.bodies{1}.draw(); 79 | this.joints{1}.draw(); 80 | title(sprintf('t = %.4f', t)); 81 | drawnow; 82 | end 83 | end 84 | 85 | %% 86 | methods (Static) 87 | %% 88 | function clear() 89 | global nm nr countCM CM; 90 | nm = 0; 91 | nr = 0; 92 | countCM = 1; 93 | if ~usejava('jvm') 94 | CM = zeros(1,3); 95 | else 96 | CM = colormap('lines'); 97 | end 98 | end 99 | 100 | %% 101 | function out = countM(data) 102 | global nm; 103 | if nargin 104 | nm = data; 105 | end 106 | if isempty(nm) 107 | out = 0; 108 | else 109 | out = nm; 110 | end 111 | end 112 | 113 | %% 114 | function out = countR(data) 115 | global nr; 116 | if nargin 117 | nr = data; 118 | end 119 | if isempty(nr) 120 | out = 0; 121 | else 122 | out = nr; 123 | end 124 | end 125 | end 126 | end 127 | -------------------------------------------------------------------------------- /c++/PCG/resources/syntax.txt: -------------------------------------------------------------------------------- 1 | Links are created one by one with respect to a parent link 2 | The first link should not have a parent link and will act as the grounded link 3 | Links must be created in order (you cannot attach a link to one that does not exist yet) 4 | To specify a linkage in input.txt follow the format below 5 | #link 6 | name string # required 7 | dens double # required - the density g/(cm*cm) 8 | size double double double # required 9 | prnt string # if this is left out, or left blank, this link will be a root linkage- there should only be one root 10 | cpos double double double # required, if the block does not have a parent then this defines the global position of the link 11 | ppos double double double # translates the joint position from the origin of the parent link 12 | angl double # defined as a multiple of pi, e.g. will produce a 90 degree clockwise rotation 13 | type TJype # defaults to ball joint 14 | # if this token appears the block will be set to invisible (in the link-by-link scheme often the root link should be invisible) 15 | # if physics appears on a line the corresponding linkage will have a disconnected inertial block 16 | angl double # if defined after the physics token this will rotate the inertial block 17 | axis double double double # axis of rotation 18 | dirn # direction of effect 19 | cpos double double double # if defined after the physics token this will translate the inertial block 20 | stif # joint stiffness 21 | damp # joint damping 22 | bdmp # body damping 23 | 24 | #spring 25 | lnka string # required 26 | lnkb string # required 27 | posa double double double # required, defaults to zero (center of mass) 28 | posb double double double # required, defaults to zero (center of mass) 29 | rest double # defaults to current distance (calculated on the fly) 30 | stif double # defaults to 0 31 | damp double # defaults to 0 32 | 33 | #closejoint 34 | lnka string # required 35 | lnkb string # required 36 | posa double double double # required, defaults to zero (center of mass) 37 | posb double double double # required, defaults to zero (center of mass) 38 | axis double double double # axis of rotation 39 | rest double # rest angle 40 | stif double # defaults to 0 41 | damp double # defaults to 0 42 | 43 | #springpoint 44 | lnka string # required 45 | posa double double double # required, defaults to zero (center of mass) 46 | axis double double double # axis of rotation 47 | stif double # defaults to 0 48 | 49 | #spline # currently only supports one spline**** 50 | prnt string # required, every spline must be matched to a point on a joint 51 | type string # defaults to cubic b-spline 52 | ppos double double double # translates the ciritical position from the origin of the parent link 53 | double double double # adds a critical point to the spline definition -- must be in order -------------------------------------------------------------------------------- /c++/PCG/src/JointUniversal.cpp: -------------------------------------------------------------------------------- 1 | #include "JointHinge.h" 2 | 3 | #include "Block.h" 4 | #include "LinkageSystem.h" 5 | #include "State.h" 6 | #include "StateDeriv.h" 7 | #include "StateSolve.h" 8 | #include "JointUniversal.h" 9 | 10 | JointUniversal::JointUniversal(std::unique_ptr& S, std::string pn, std::string cn, Eigen::Vector3d pp, Eigen::Vector3d cp, bool r, int i, double angleX, double angleY, double k, double d) : 11 | Joint(S, JType::Universal, pn, cn, pp, cp, r, i, 2, k, d) 12 | { 13 | this->axis = axis; 14 | 15 | double c1 = std::cos(angleX); 16 | double c2 = std::cos(angleY); 17 | double s1 = std::sin(k); 18 | double s2 = std::sin(d); 19 | 20 | this->S = Eigen::MatrixXd::Zero(6, 2); 21 | this->S(0,0) = c2; 22 | this->S(2,0) = s2; 23 | this->S(1,1) = 1; 24 | this->ST = this->S.transpose(); 25 | Sdot = Eigen::MatrixXd::Zero(6, 2); 26 | 27 | q0[0] = angleX; 28 | q0[1] = angleY; 29 | //qdot0[0] = k; 30 | //qdot0[1] = d; 31 | //V[0] = 1; 32 | //V[1] = 1; 33 | 34 | Q = Eigen::Matrix4d::Identity(); 35 | Q(0, 0) = c2; 36 | Q(0, 1) = s1*c2; 37 | Q(0, 2) = -c1*s2; 38 | Q(1, 0) = 0; 39 | Q(1, 1) = c1; 40 | Q(1, 2) = s1; 41 | Q(2, 0) = s2; 42 | Q(2, 1) = -s1*c2; 43 | Q(2, 2) = c1*c2; 44 | 45 | Pr = Eigen::MatrixXd::Zero(1, 1); 46 | Psi = Eigen::MatrixXd::Zero(1, 1); 47 | Kmd = Matrix6d::Zero(); 48 | Dmd = Matrix6d::Zero(); 49 | } 50 | 51 | void JointUniversal::updateMaxSparse(std::unique_ptr& SS, const std::unique_ptr& LS, const std::unique_ptr& S, Matrix6d & parentAdj, Matrix6d & childAdj, Eigen::Vector3d & g) const 52 | { 53 | } 54 | 55 | void JointUniversal::mapSelf(int c_i, std::unique_ptr& S) 56 | { 57 | new (&q) Eigen::Map(&(S->q[c_i]), constraintNum); 58 | q[0] = q0[0]; 59 | q[1] = q0[1]; 60 | 61 | new (&qdot) Eigen::Map(&(S->qdot[c_i]), constraintNum); 62 | qdot[0] = qdot0[0]; 63 | qdot[1] = qdot0[1]; 64 | 65 | updateSelf(); 66 | } 67 | 68 | void JointUniversal::updateSelf() 69 | { 70 | double c1, c2, s1, s2; 71 | if (q.size() != 0) 72 | { 73 | c1 = std::cos(q[0]); 74 | c2 = std::cos(q[1]); 75 | s1 = std::sin(q[0]); 76 | s2 = std::sin(q[1]); 77 | } 78 | else 79 | { 80 | c1 = std::cos(q0[0]); 81 | c2 = std::cos(q0[1]); 82 | s1 = std::sin(q0[0]); 83 | s2 = std::sin(q0[1]); 84 | } 85 | double qd1, qd2; 86 | if (qdot.size() != 0) 87 | { 88 | qd1 = qdot[0]; 89 | qd2 = qdot[1]; 90 | } 91 | else 92 | { 93 | qd1 = qdot0[0]; 94 | qd2 = qdot0[1]; 95 | } 96 | 97 | Q(0, 0) = c2; 98 | Q(1, 0) = s1 * s2; 99 | Q(2, 0) = -c1 * s2; 100 | Q(0, 1) = 0; 101 | Q(1, 1) = c1; 102 | Q(2, 1) = s1; 103 | Q(0, 2) = s2; 104 | Q(1, 2) = -s1 * c2; 105 | Q(2, 2) = c1 * c2; 106 | 107 | S(0, 0) = c2; 108 | S(2, 0) = s2; 109 | Sdot(0, 0) = -s2 * qd1; 110 | Sdot(2, 0) = c2 * qd2; 111 | 112 | ST = S.transpose(); 113 | } 114 | 115 | Vector6d JointUniversal::Sqdot(const std::unique_ptr& S) 116 | { 117 | return this->S * qdot; 118 | } 119 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/JointRevolute.m: -------------------------------------------------------------------------------- 1 | classdef JointRevolute < redmax.Joint 2 | %% 3 | properties 4 | axis % Axis of rotation 5 | radius % Radius for contact 6 | height % Height for contact 7 | end 8 | 9 | %% 10 | methods 11 | %% 12 | function this = JointRevolute(parent,body,axis) 13 | this = this@redmax.Joint(parent,body,1); 14 | this.axis = axis/norm(axis); 15 | this.radius = 1.0; 16 | this.height = 1.0; 17 | end 18 | 19 | %% 20 | function setGeometry(this,radius,height) 21 | this.radius = radius; 22 | this.height = height; 23 | end 24 | end 25 | 26 | %% 27 | methods (Access = protected) 28 | %% 29 | function update_(this,deriv) 30 | q = this.q; 31 | qdot = this.qdot; 32 | a = this.axis; 33 | 34 | R = se3.aaToMat(a,q); % faster than se3.exp(se3.brac(a*q)); 35 | this.Q(1:3,1:3) = R; 36 | this.A = se3.Ad(this.Q); 37 | this.S = [a; zeros(3,1)]; 38 | 39 | abrac = se3.brac(a); 40 | dRdq = R*abrac; 41 | Rdot = dRdq*qdot; 42 | this.Adot(1:3,1:3) = Rdot; 43 | this.Adot(4:6,4:6) = Rdot; 44 | 45 | if deriv 46 | this.dAdq(1:3,1:3) = dRdq; 47 | this.dAdq(4:6,4:6) = dRdq; 48 | d2Rdq2 = dRdq*abrac; 49 | tmp = d2Rdq2*qdot; 50 | this.dAdotdq(1:3,1:3) = tmp; 51 | this.dAdotdq(4:6,4:6) = tmp; 52 | end 53 | end 54 | 55 | %% 56 | function draw_(this) 57 | n = 8; 58 | E_ja = eye(4); 59 | z = [0 0 1]'; 60 | angle = acos(max(-1.0, min(this.axis'*z, 1.0))); 61 | E_ja(1:3,1:3) = se3.aaToMat(cross(this.axis,z),angle); 62 | E = this.E_wj*E_ja; 63 | % Cylinder 64 | r = this.radius; 65 | h = this.height; 66 | S = diag([1,1,h,1]); 67 | T = [eye(3),[0 0 -h/2]'; 0 0 0 1]; 68 | [X,Y,Z] = cylinder(r,n); 69 | X = reshape(X,1,2*(n+1)); 70 | Y = reshape(Y,1,2*(n+1)); 71 | Z = reshape(Z,1,2*(n+1)); 72 | XYZ = E*T*S*[X;Y;Z;ones(1,2*(n+1))]; 73 | X0 = reshape(XYZ(1,:),2,n+1); 74 | Y0 = reshape(XYZ(2,:),2,n+1); 75 | Z0 = reshape(XYZ(3,:),2,n+1); 76 | %surf(X,Y,Z,'FaceColor',this.body.color); 77 | % Ends 78 | % https://www.mathworks.com/matlabcentral/answers/320174-plot-a-surface-on-a-circle-and-annulus 79 | [T,R] = meshgrid(linspace(0,2*pi,n+1),linspace(0,r,2)); 80 | X = R.*cos(T); 81 | Y = R.*sin(T); 82 | Z = -h/2*ones(size(R)); 83 | X = reshape(X,1,2*(n+1)); 84 | Y = reshape(Y,1,2*(n+1)); 85 | Z = reshape(Z,1,2*(n+1)); 86 | XYZ = E*[X;Y;Z;ones(1,2*(n+1))]; 87 | X1 = reshape(XYZ(1,:),2,n+1); 88 | Y1 = reshape(XYZ(2,:),2,n+1); 89 | Z1 = reshape(XYZ(3,:),2,n+1); 90 | %surf(X,Y,Z,'FaceColor',this.body.color); 91 | X = R.*cos(T); 92 | Y = R.*sin(T); 93 | Z = h/2*ones(size(R)); 94 | X = reshape(X,1,2*(n+1)); 95 | Y = reshape(Y,1,2*(n+1)); 96 | Z = reshape(Z,1,2*(n+1)); 97 | XYZ = E*[X;Y;Z;ones(1,2*(n+1))]; 98 | X2 = reshape(XYZ(1,:),2,n+1); 99 | Y2 = reshape(XYZ(2,:),2,n+1); 100 | Z2 = reshape(XYZ(3,:),2,n+1); 101 | surf([X0 X1 X2],[Y0 Y1 Y2],[Z0 Z1 Z2],'FaceColor',this.body.color); 102 | end 103 | end 104 | end 105 | -------------------------------------------------------------------------------- /matlab-simple/testRedMax.m: -------------------------------------------------------------------------------- 1 | function testRedMax(sceneID) 2 | % testRedMax Reference implementation of the Red/Max algorithm 3 | % 4 | % sceneID: What scene to run. 5 | % 0: Simple serial chain 6 | % 1: Different revolute axes 7 | % 2: Branching 8 | 9 | if nargin < 1 10 | sceneID = 0; 11 | end 12 | 13 | scene = testRedMaxScenes(sceneID); 14 | scene.init(); 15 | 16 | % Draw initial scene 17 | if scene.drawHz ~= 0 18 | scene.draw(0); 19 | end 20 | fprintf('(%d) ''%s'': tspan = [%.1f %.1f]: nr=%d, nm=%d\n',... 21 | sceneID,scene.name,scene.tspan,... 22 | redmax.Scene.countR(),redmax.Scene.countM()); 23 | 24 | % Integrate 25 | tic 26 | [t,y] = euler(scene); 27 | toc 28 | fprintf('%d steps\n',length(t)); 29 | 30 | % Draw 31 | dt = scene.tspan(2) - scene.tspan(1); 32 | if scene.drawHz ~= 0 && dt > 0 33 | for tk = scene.tspan(1) : 1/scene.drawHz : scene.tspan(2) 34 | [k,s] = searchTime(t,tk); 35 | ys = (1-s)*y(k,:) + s*y(k+1,:); 36 | scene.joints{1}.scatterDofs(ys); 37 | scene.draw(tk); 38 | end 39 | end 40 | 41 | end 42 | 43 | %% 44 | function [i,s] = searchTime(t,ti) 45 | % Finds the index of the time interval around ti 46 | tis = find(t < ti); 47 | if isempty(tis) 48 | % Beginning of time 49 | i = 1; 50 | s = 0; 51 | else 52 | i = tis(end); 53 | if i == length(t) 54 | % End of time 55 | i = i - 1; 56 | s = 1; 57 | else 58 | % Somewhere in the middle 59 | t0 = t(i); 60 | t1 = t(i + 1); 61 | s = (ti - t0)/(t1 - t0); 62 | end 63 | end 64 | end 65 | 66 | %% 67 | function [t,y] = euler(scene) 68 | nr = redmax.Scene.countR(); 69 | body0 = scene.bodies{1}; 70 | joint0 = scene.joints{1}; 71 | % Allocate t and y 72 | % t(k) is the time at step k 73 | % y(k,:) = [q;qdot]' is the position and velocity at step k 74 | h = scene.hEuler; 75 | t = scene.tspan(1) : h : scene.tspan(2); 76 | y = zeros(length(t),2*nr); 77 | y1 = joint0.gatherDofs(); 78 | y(1,:) = y1; 79 | % Integrate 80 | for k = 2 : length(t) 81 | % Mm: maximal mass matrix 82 | % fm: maximal force vector (eg gravity) 83 | % Km: maximal stiffness matrix (eg springs, unused in matlab-simple) 84 | % Dm: maximal damping matrix (eg body damping) 85 | % fr: reduced force vector (eg external joint torques) 86 | % Kr: reduced stiffness matrix (eg joint stiffness) 87 | % Dr: reduced damping matrix (eg joint damping) 88 | % J, Jdot: Jacobian and its time derivative 89 | [Mm,fm] = body0.computeMassGrav(scene.grav); 90 | [fm,Dm] = body0.computeForceDamping(fm); 91 | [fr,Kr] = joint0.computeForceStiffness(); 92 | [~,Dr] = joint0.computeForceDamping(); 93 | [J,Jdot] = joint0.computeJacobian(); 94 | q0 = y(k-1,1:nr)'; 95 | qdot0 = y(k-1,nr+1:end)'; 96 | Mr = J'*Mm*J; 97 | Mr = 0.5*(Mr + Mr'); 98 | frtilde = Mr*qdot0 + h*(J'*(fm - Mm*Jdot*qdot0) + fr); 99 | Mrtilde = Mr + J'*h*Dm*J + h*Dr - h*h*Kr; 100 | qdot1 = Mrtilde\frtilde; 101 | qddot = (qdot1 - qdot0)/h; 102 | q1 = q0 + h*qdot1; 103 | yk = [q1;qdot1]; 104 | ydotk = [qdot1;qddot]; 105 | joint0.scatterDofs(yk); 106 | joint0.scatterDDofs(ydotk); 107 | y(k,:) = yk; 108 | end 109 | end 110 | -------------------------------------------------------------------------------- /c++/PCG/src/online/Program.cpp: -------------------------------------------------------------------------------- 1 | #include "Program.h" 2 | 3 | #include 4 | #include 5 | 6 | #include "GLSL.h" 7 | 8 | using namespace std; 9 | 10 | Program::Program() : 11 | vShaderName(""), 12 | fShaderName(""), 13 | pid(0), 14 | verbose(true) 15 | { 16 | 17 | } 18 | 19 | Program::~Program() 20 | { 21 | 22 | } 23 | 24 | void Program::setShaderNames(const string &v, const string &f) 25 | { 26 | vShaderName = v; 27 | fShaderName = f; 28 | } 29 | 30 | bool Program::init() 31 | { 32 | GLint rc; 33 | 34 | // Create shader handles 35 | GLuint VS = glCreateShader(GL_VERTEX_SHADER); 36 | GLuint FS = glCreateShader(GL_FRAGMENT_SHADER); 37 | 38 | // Read shader sources 39 | const char *vshader = GLSL::textFileRead(vShaderName.c_str()); 40 | const char *fshader = GLSL::textFileRead(fShaderName.c_str()); 41 | glShaderSource(VS, 1, &vshader, NULL); 42 | glShaderSource(FS, 1, &fshader, NULL); 43 | 44 | // Compile vertex shader 45 | glCompileShader(VS); 46 | glGetShaderiv(VS, GL_COMPILE_STATUS, &rc); 47 | if(!rc) { 48 | if(isVerbose()) { 49 | GLSL::printShaderInfoLog(VS); 50 | cout << "Error compiling vertex shader " << vShaderName << endl; 51 | } 52 | return false; 53 | } 54 | 55 | // Compile fragment shader 56 | glCompileShader(FS); 57 | glGetShaderiv(FS, GL_COMPILE_STATUS, &rc); 58 | if(!rc) { 59 | if(isVerbose()) { 60 | GLSL::printShaderInfoLog(FS); 61 | cout << "Error compiling fragment shader " << fShaderName << endl; 62 | } 63 | return false; 64 | } 65 | 66 | // Create the program and link 67 | pid = glCreateProgram(); 68 | glAttachShader(pid, VS); 69 | glAttachShader(pid, FS); 70 | glLinkProgram(pid); 71 | glGetProgramiv(pid, GL_LINK_STATUS, &rc); 72 | if(!rc) { 73 | if(isVerbose()) { 74 | GLSL::printProgramInfoLog(pid); 75 | cout << "Error linking shaders " << vShaderName << " and " << fShaderName << endl; 76 | } 77 | return false; 78 | } 79 | 80 | GLSL::checkError(GET_FILE_LINE); 81 | return true; 82 | } 83 | 84 | void Program::bind() 85 | { 86 | glUseProgram(pid); 87 | } 88 | 89 | void Program::unbind() 90 | { 91 | glUseProgram(0); 92 | } 93 | 94 | void Program::addAttribute(const string &name) 95 | { 96 | attributes[name] = glGetAttribLocation(pid, name.c_str()); 97 | } 98 | 99 | void Program::addUniform(const string &name) 100 | { 101 | uniforms[name] = glGetUniformLocation(pid, name.c_str()); 102 | } 103 | 104 | GLint Program::getAttribute(const string &name) const 105 | { 106 | map::const_iterator attribute = attributes.find(name.c_str()); 107 | if(attribute == attributes.end()) { 108 | if(isVerbose()) { 109 | cout << name << " is not an attribute variable" << endl; 110 | } 111 | return -1; 112 | } 113 | return attribute->second; 114 | } 115 | 116 | GLint Program::getUniform(const string &name) const 117 | { 118 | map::const_iterator uniform = uniforms.find(name.c_str()); 119 | if(uniform == uniforms.end()) { 120 | if(isVerbose()) { 121 | cout << name << " is not a uniform variable" << endl; 122 | } 123 | return -1; 124 | } 125 | return uniform->second; 126 | } 127 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/TaskBDF2.m: -------------------------------------------------------------------------------- 1 | classdef (Abstract) TaskBDF2 < handle 2 | %TaskBDF2 A user-defined objective to be minimized 3 | 4 | %% 5 | properties 6 | scene 7 | p % parameters 8 | P % objective value 9 | wreg % regularizer weight 10 | dgdp % 11 | dPdq % 12 | end 13 | 14 | %% 15 | methods 16 | %% 17 | function this = TaskBDF2(scene,nparams) 18 | this.scene = scene; 19 | this.p = zeros(nparams,1); 20 | this.P = 0; 21 | this.wreg = 1; 22 | this.dgdp = []; 23 | this.dPdq = {}; 24 | end 25 | 26 | %% Initializes the task 27 | function init(this) 28 | nr = redmax.Scene.countR(); 29 | np = length(this.p); 30 | this.P = 0; 31 | nsteps = this.scene.nsteps; 32 | this.dPdq = cell(1,nsteps); 33 | this.dgdp = sparse(nsteps*nr,np); 34 | end 35 | 36 | %% Applies the parameters to the current step 37 | function applyStep(this) %#ok 38 | end 39 | 40 | %% Computes objective value and derivatives after a sim step 41 | function calcStep(this) %#ok 42 | end 43 | 44 | %% Computes objective value and derivatives after the sim 45 | function [P,dPdp] = calcFinal(this) 46 | nr = redmax.Scene.countR(); 47 | 48 | % Assume that all tasks have a simple regularizer 49 | P = this.P + this.wreg*0.5*(this.p'*this.p); 50 | 51 | % Compute derivative 52 | % We are not computing dg/dqa, assuming that dP/dqa is 0. In 53 | % other words, we're assuming that the objective does not 54 | % depend on time step alpha. 55 | nsteps = this.scene.nsteps; 56 | z = zeros(nsteps*nr,1); 57 | h = this.scene.h; 58 | a = (2-sqrt(2))/2; 59 | for k = this.scene.nsteps : -1 : 1 60 | yk = this.dPdq{k}; 61 | kk0 = (k-1)*nr + (1:nr); % indices for time step k 62 | kk1 = kk0 + nr; % indices for time step k+1 63 | kk2 = kk1 + nr; % indices for time step k+2 64 | kk3 = kk2 + nr; % indices for time step k+3 65 | kk4 = kk3 + nr; % indices for time step k+4 66 | if k < nsteps 67 | % Add contributions from step k+1 68 | M = this.scene.history(k+1).M; 69 | D = this.scene.history(k+1).D; 70 | if k == 1 71 | block = -((8/(9*a)) + (4/3))*M + (8/9)*h*D; 72 | else 73 | block = -(8/3)*M + (8/9)*h*D; 74 | end 75 | yk = yk - block'*z(kk1); 76 | end 77 | if k < nsteps - 1 78 | % Add contributions from step k+2 79 | M = this.scene.history(k+2).M; 80 | D = this.scene.history(k+2).D; 81 | if k == 1 82 | block = ((2/(9*a)) + (19/9))*M - (2/9)*h*D; 83 | else 84 | block = (22/9)*M - (2/9)*h*D; 85 | end 86 | yk = yk - block'*z(kk2); 87 | end 88 | if k < nsteps - 2 89 | % Add contributions from step k+3 90 | M = this.scene.history(k+3).M; 91 | block = -(8/9)*M; 92 | yk = yk - block'*z(kk3); 93 | end 94 | if k < nsteps - 3 95 | % Add contributions from step k+4 96 | M = this.scene.history(k+4).M; 97 | block = (1/9)*M; 98 | yk = yk - block'*z(kk4); 99 | end 100 | % Solve by the diagonal 101 | Hl = this.scene.history(k).Hl; 102 | Hu = this.scene.history(k).Hu; 103 | Hp = this.scene.history(k).Hp; 104 | zkk0(Hp) = Hl'\(Hu'\yk); 105 | z(kk0) = zkk0; 106 | end 107 | dPdp = this.wreg*this.p' - z'*this.dgdp; 108 | end 109 | 110 | %% Draws the current task state 111 | function draw(this) %#ok 112 | end 113 | end 114 | end 115 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/TaskBDF1PointPos.m: -------------------------------------------------------------------------------- 1 | classdef TaskBDF1PointPos < redmax.TaskBDF1 2 | %TaskBDF1PointPos Sets a point on a rigid body to be at a target point. 3 | % The parameters are the fixed torques on all of the joints. 4 | 5 | %% 6 | properties 7 | t % time at which the measurement will be taken 8 | body % body to move to the target 9 | xlocal % the local point on the body 10 | xtarget % the world target position 11 | pscale % scale factor for the torques 12 | wpos % position weight 13 | end 14 | 15 | %% 16 | methods 17 | %% 18 | function this = TaskBDF1PointPos(scene) 19 | nparams = 0; 20 | for i = 1 : length(scene.joints) 21 | nparams = nparams + scene.joints{i}.ndof; 22 | end 23 | this = this@redmax.TaskBDF1(scene,nparams); 24 | end 25 | 26 | %% 27 | function setTime(this,t) 28 | this.t = t; 29 | end 30 | 31 | %% 32 | function setBody(this,body) 33 | this.body = body; 34 | end 35 | 36 | %% 37 | function setPoint(this,xlocal) 38 | this.xlocal = xlocal; 39 | end 40 | 41 | %% 42 | function setTarget(this,xtarget) 43 | this.xtarget = xtarget; 44 | end 45 | 46 | %% 47 | function setScale(this,pscale) 48 | this.pscale = pscale; 49 | end 50 | 51 | %% 52 | function setWeights(this,wreg,wpos) 53 | this.wreg = wreg; 54 | this.wpos = wpos; 55 | end 56 | 57 | %% Applies the parameters to the current step 58 | function applyStep(this) 59 | % The indexing here assumes that we are controlling all of the 60 | % joints in the scene 61 | for i = 1 : length(this.scene.joints) 62 | this.scene.joints{i}.tau = this.pscale*this.p(this.scene.joints{i}.idxR); 63 | end 64 | end 65 | 66 | %% Computes objective value and derivatives after a sim step 67 | function calcStep(this) 68 | nm = redmax.Scene.countM(); 69 | nr = redmax.Scene.countR(); 70 | 71 | wp = this.wpos; 72 | 73 | k = this.scene.k; 74 | dt = this.t - this.scene.t; 75 | if abs(dt) < 1e-6 76 | % Objective 77 | q = this.scene.history(k).q; 78 | qdot = this.scene.history(k).qdot; 79 | this.scene.joints{1}.setQ(q,qdot); 80 | this.scene.joints{1}.update(); 81 | E = this.body.E_wi; 82 | xworld = E(1:3,:)*[this.xlocal;1]; 83 | dx = xworld - this.xtarget; 84 | this.P = this.P + wp*0.5*(dx'*dx); 85 | 86 | % Derivative 87 | % dPdq = dP/dx * dx/dqm * dqm/dqr 88 | % = wp*dx * R*G * J 89 | dxdqm = zeros(3,nm); 90 | R = E(1:3,1:3); 91 | dxdqm(:,this.body.idxM) = R*se3.Gamma(this.xlocal); 92 | J = this.scene.history(k).J; 93 | this.dPdq{k} = J'*dxdqm'*dx*wp; 94 | else 95 | this.dPdq{k} = zeros(nr,1); 96 | end 97 | 98 | % dg/dp: the derivative of Newton function g wrt the parameters 99 | % With BDF1: 100 | % g = M*(q1 - q0 - h*qdot0) - h^2*(fr + J'*fm + fqvv) 101 | % The parameters are the torques scaled by pscale, which go 102 | % into fr, so: 103 | % dg/dp = -h^2*pscale*I 104 | h = this.scene.h; 105 | kk = (k-1)*nr + (1:nr); % indices for time step k 106 | this.dgdp(kk,:) = -h^2*this.pscale*speye(nr); 107 | end 108 | 109 | %% Draws the current task state 110 | function draw(this) 111 | x1 = this.xtarget; 112 | plot3(x1(1),x1(2),x1(3),'gs'); 113 | xw = this.body.E_wi*[this.xlocal;1]; 114 | xw1 = [xw(1:3),x1]; 115 | plot3(xw1(1,:),xw1(2,:),xw1(3,:),'g'); 116 | end 117 | end 118 | 119 | %% 120 | methods (Access = protected) 121 | end 122 | end 123 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/TaskBDF2PointPos.m: -------------------------------------------------------------------------------- 1 | classdef TaskBDF2PointPos < redmax.TaskBDF2 2 | %TaskBDF2PointPos Sets a point on a rigid body to be at a target point. 3 | % The parameters are the fixed torques on all of the joints. 4 | 5 | %% 6 | properties 7 | t % time at which the measurement will be taken 8 | body % body to move to the target 9 | xlocal % the local point on the body 10 | xtarget % the world target position 11 | pscale % scale factor for the torques 12 | wpos % position weight 13 | end 14 | 15 | %% 16 | methods 17 | %% 18 | function this = TaskBDF2PointPos(scene) 19 | nparams = 0; 20 | for i = 1 : length(scene.joints) 21 | nparams = nparams + scene.joints{i}.ndof; 22 | end 23 | this = this@redmax.TaskBDF2(scene,nparams); 24 | end 25 | 26 | %% 27 | function setTime(this,t) 28 | this.t = t; 29 | end 30 | 31 | %% 32 | function setBody(this,body) 33 | this.body = body; 34 | end 35 | 36 | %% 37 | function setPoint(this,xlocal) 38 | this.xlocal = xlocal; 39 | end 40 | 41 | %% 42 | function setTarget(this,xtarget) 43 | this.xtarget = xtarget; 44 | end 45 | 46 | %% 47 | function setScale(this,pscale) 48 | this.pscale = pscale; 49 | end 50 | 51 | %% 52 | function setWeights(this,wreg,wpos) 53 | this.wreg = wreg; 54 | this.wpos = wpos; 55 | end 56 | 57 | %% Applies the parameters to the current step 58 | function applyStep(this) 59 | % The indexing here assumes that we are controlling all of the 60 | % joints in the scene 61 | for i = 1 : length(this.scene.joints) 62 | this.scene.joints{i}.tau = this.pscale*this.p(this.scene.joints{i}.idxR); 63 | end 64 | end 65 | 66 | %% Computes objective value and derivatives after a sim step 67 | function calcStep(this) 68 | nm = redmax.Scene.countM(); 69 | nr = redmax.Scene.countR(); 70 | 71 | wp = this.wpos; 72 | 73 | k = this.scene.k; 74 | dt = this.t - this.scene.t; 75 | if abs(dt) < 1e-6 76 | % Objective 77 | q = this.scene.history(k).q; 78 | qdot = this.scene.history(k).qdot; 79 | this.scene.joints{1}.setQ(q,qdot); 80 | this.scene.joints{1}.update(); 81 | E = this.body.E_wi; 82 | xworld = E(1:3,:)*[this.xlocal;1]; 83 | dx = xworld - this.xtarget; 84 | this.P = this.P + wp*0.5*(dx'*dx); 85 | 86 | % Derivative 87 | % dPdq = dP/dx * dx/dqm * dqm/dqr 88 | % = wp*dx * R*G * J 89 | dxdqm = zeros(3,nm); 90 | R = E(1:3,1:3); 91 | dxdqm(:,this.body.idxM) = R*se3.Gamma(this.xlocal); 92 | J = this.scene.history(k).J; 93 | this.dPdq{k} = J'*dxdqm'*dx*wp; 94 | else 95 | this.dPdq{k} = zeros(nr,1); 96 | end 97 | 98 | % dg/dp: the derivative of Newton function g wrt the parameters 99 | % With BDF2, the force term of g is: 100 | % g = ... - (4/9)*h^2*(fr + J'*fm + fqvv) 101 | % The parameters are the torques scaled by pscale, which go 102 | % into fr, so: 103 | % dg/dp = -(4/9)*h^2*pscale*I 104 | h = this.scene.h; 105 | kk = (k-1)*nr + (1:nr); % indices for time step k 106 | this.dgdp(kk,:) = -(4/9)*h^2*this.pscale*speye(nr); 107 | end 108 | 109 | %% Draws the current task state 110 | function draw(this) 111 | x1 = this.xtarget; 112 | plot3(x1(1),x1(2),x1(3),'gs'); 113 | xw = this.body.E_wi*[this.xlocal;1]; 114 | xw1 = [xw(1:3),x1]; 115 | plot3(xw1(1,:),xw1(2,:),xw1(3,:),'g'); 116 | end 117 | end 118 | 119 | %% 120 | methods (Access = protected) 121 | end 122 | end 123 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/JointFree3D.m: -------------------------------------------------------------------------------- 1 | classdef JointFree3D < redmax.Joint 2 | % 3D free joint 3 | % This composite joint uses JointTranslational as S1 and 4 | % JointSpherical as S2. 5 | % 6 | 7 | %% 8 | properties 9 | joint1 10 | joint2 11 | end 12 | 13 | %% 14 | methods 15 | %% 16 | function this = JointFree3D(parent,body) 17 | this = this@redmax.Joint(parent,body,6); 18 | this.joint1 = redmax.JointTranslational([],body); 19 | this.joint2 = redmax.JointSpherical([],body); 20 | body.joint = this; 21 | end 22 | end 23 | 24 | %% 25 | methods (Access = protected) 26 | %% 27 | function reparam_(this) 28 | this.joint2.reparam_(); 29 | this.q(4:6) = this.joint2.q; 30 | this.qdot(4:6) = this.joint2.qdot; 31 | end 32 | 33 | %% 34 | function update_(this,deriv) 35 | this.joint1.q = this.q(1:3); 36 | this.joint2.q = this.q(4:6); 37 | this.joint1.qdot = this.qdot(1:3); 38 | this.joint2.qdot = this.qdot(4:6); 39 | this.joint1.update_(deriv); 40 | this.joint2.update_(deriv); 41 | 42 | % Extract from translational joint 43 | p = this.joint1.q; 44 | pdot = this.joint1.qdot; 45 | 46 | % Extract from spherical joint 47 | rdot = this.joint2.qdot; 48 | R = this.joint2.Q(1:3,1:3); 49 | Rdot = this.joint2.Adot(1:3,1:3); 50 | T = this.joint2.S(1:3,1:3); 51 | Tdot = this.joint2.Sdot(1:3,1:3); 52 | dRdr = zeros(3,3,3); 53 | dRdotdr = zeros(3,3,3); 54 | dTdr = zeros(3,3,3); 55 | dTdotdr = zeros(3,3,3); 56 | for k = 1 : 3 57 | dRdr(:,:,k) = this.joint2.dAdq(1:3,1:3,k); 58 | dRdotdr(:,:,k) = this.joint2.dAdotdq(1:3,1:3,k); 59 | dTdr(:,:,k) = this.joint2.dSdq(1:3,:,k); 60 | dTdotdr(:,:,k) = this.joint2.dSdotdq(1:3,:,k); 61 | end 62 | 63 | % Q and A 64 | this.Q(1:3,1:3) = R; 65 | this.Q(1:3,4) = p; 66 | this.A(1:3,1:3) = R; 67 | this.A(4:6,4:6) = R; 68 | pbrac = se3.brac(p); 69 | this.A(4:6,1:3) = pbrac*R; 70 | 71 | % Adot 72 | pdotbrac = se3.brac(pdot); 73 | this.Adot(1:3,1:3) = Rdot; 74 | this.Adot(4:6,4:6) = Rdot; 75 | this.Adot(4:6,1:3) = pdotbrac*R + pbrac*Rdot; 76 | 77 | % S 78 | this.S(4:6,1:3) = R'; 79 | this.S(1:3,4:6) = T; 80 | 81 | % Sdot 82 | this.Sdot(4:6,1:3) = Rdot'; 83 | this.Sdot(1:3,4:6) = Tdot; 84 | 85 | if deriv 86 | % dAdq 87 | for k = 1 : 3 88 | ek = zeros(3,1); 89 | ek(k) = 1; 90 | ekbrac = se3.brac(ek); 91 | dRdrk = dRdr(:,:,k); 92 | this.dAdq(1:3,1:3,3+k) = dRdrk; 93 | this.dAdq(4:6,4:6,3+k) = dRdrk; 94 | this.dAdq(4:6,1:3,k) = ekbrac*R; 95 | this.dAdq(4:6,1:3,3+k) = pbrac*dRdrk; 96 | end 97 | 98 | % dAdotdq 99 | for k = 1 : 3 100 | ek = zeros(3,1); 101 | ek(k) = 1; 102 | ekbrac = se3.brac(ek); 103 | dRdotdrk = dRdotdr(:,:,k); 104 | this.dAdotdq(4:6,1:3,k) = ekbrac*Rdot; 105 | this.dAdotdq(1:3,1:3,3+k) = dRdotdrk; 106 | this.dAdotdq(4:6,4:6,3+k) = dRdotdrk; 107 | this.dAdotdq(4:6,1:3,3+k) = pdotbrac*dRdr(:,:,k) + pbrac*dRdotdrk; 108 | end 109 | 110 | % dSdq 111 | for k = 1 : 3 112 | dRdrk = dRdr(:,:,k); 113 | this.dSdq(4:6,1:3,3+k) = dRdrk'; 114 | this.dSdq(1:3,4:6,3+k) = dTdr(:,:,k); 115 | end 116 | 117 | % dSdotdq 118 | tmp = se3.brac(T*rdot); 119 | for k = 1 : 3 120 | this.dSdotdq(4:6,1:3,3+k) = -se3.brac(dTdr(:,:,k)*rdot)*R' - tmp*dRdr(:,:,k)'; 121 | this.dSdotdq(1:3,4:6,3+k) = dTdotdr(:,:,k); 122 | end 123 | end 124 | end 125 | end 126 | end 127 | -------------------------------------------------------------------------------- /c++/PCG/src/State.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "Rigid.h" 4 | #include "Joint.h" 5 | 6 | #define EIGEN_DONT_ALIGN_STATICALLY 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | struct Block; 13 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix4d) 14 | EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix6d) 15 | // -----------------------------------------------------// 16 | // Structure of Array style data container for Linkage // 17 | // System State information // 18 | // -----------------------------------------------------// 19 | struct State 20 | { 21 | 22 | // time in the simulation 23 | double &t; 24 | // Velocity vector - maximal -- phi_ii 25 | Eigen::VectorXd v; 26 | // Transformation matrix E -- E_wi 27 | std::vector E; 28 | // Joint positions - reduced 29 | Eigen::VectorXd q; 30 | // Velocity vector - reduced 31 | Eigen::VectorXd qdot; 32 | // Joint accelerations 33 | Eigen::VectorXd qddot; 34 | 35 | // Allow for the inertial matricies to have different sizes 36 | // std::vector size; 37 | // Mass vector 38 | std::vector mass; 39 | // Spatial inertia: M 40 | std::vector M; 41 | 42 | 43 | ///*********************************************************/ 44 | ///////////////////////////////////////////////////////////// 45 | // 1/29 CHECK ALL ON-THE-FLY ARE NOT SUPERCEDED INCORRECTLY 46 | ///////////////////////////////////////////////////////////// 47 | ///*********************************************************/ 48 | 49 | /// Reduced/Maximal PCG SoA 50 | // SoA constants 51 | std::vector jindex; 52 | std::vector type; 53 | std::vector constraintNum; 54 | std::vector constraint_index; 55 | std::vector parentIndex; 56 | std::vector> childIndex; 57 | std::vector root; 58 | std::vector damping; 59 | std::vector d; 60 | std::vector k; 61 | 62 | // Joints 63 | std::vector V; 64 | std::vector S; 65 | std::vector ST; 66 | std::vector Sdot; 67 | std::vector q0; 68 | 69 | std::vector I_j; 70 | 71 | std::vector ad_ij; 72 | std::vector ad_ijT; 73 | std::vector ad_jp; 74 | std::vector ad_jpT; 75 | 76 | std::vector addot; 77 | std::vector ad_ip; 78 | std::vector ad_ipT; 79 | std::vector add_ip; 80 | std::vector ad_iw; 81 | std::vector ad_wi; 82 | 83 | std::vector alpha; 84 | std::vector beta; 85 | 86 | std::vector Pr; 87 | std::vector Kmd; 88 | std::vector Dmd; 89 | std::vector D; 90 | std::vector Mhat; 91 | std::vector Psi; 92 | std::vector Pi; 93 | std::vector Bhat; 94 | std::vector Vdot; 95 | 96 | std::vector adij_S; 97 | std::vector ST_adijT; 98 | std::vector ST_Mhat; 99 | std::vector S_Psi; 100 | std::vector Mhat_S_Psi; 101 | std::vector ST_Bhat; 102 | 103 | // For non-joint constraints 104 | 105 | // Conjugate Gradient 106 | struct local_mt 107 | { 108 | std::vector alpha_; 109 | std::vector Bhat_; 110 | std::vector beta_; 111 | std::vector Vdot_; 112 | std::vector ST_Bhat_; 113 | }; 114 | 115 | State(double &t):t(t) 116 | {}; 117 | ~State() {}; 118 | }; 119 | -------------------------------------------------------------------------------- /c++/PCG/src/online/Brender/cpp/BrenderManager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * @author: Gustavo Lopez 10-21-17 3 | * 4 | * @version: 1.0 5 | */ 6 | 7 | #include "BrenderManager.h" 8 | #include "Brenderable.h" 9 | 10 | 11 | using namespace std; 12 | 13 | bool BrenderManager::instanceFlag_ = false; 14 | BrenderManager* BrenderManager::manager_ = NULL; 15 | BrenderManager* BrenderManager::getInstance() 16 | { 17 | if (!instanceFlag_) 18 | { 19 | manager_ = new BrenderManager(); 20 | instanceFlag_ = true; 21 | return manager_; 22 | } 23 | else 24 | { 25 | return manager_; 26 | } 27 | } 28 | 29 | int BrenderManager::getFrame() const 30 | { 31 | return frame_; 32 | } 33 | 34 | void BrenderManager::exportBrender(double time) 35 | { 36 | int objNum = 1; 37 | for (auto brenderable : brenderables_) { 38 | vector names = brenderable->getBrenderNames(); 39 | vector extensions = brenderable->getBrenderExtensions(); 40 | vector types = brenderable->getBrenderTypes(); 41 | vector< shared_ptr< ofstream > > outfiles; 42 | // Initialize files 43 | for (int i = 0; i < brenderable->getBrenderCount(); ++i) { 44 | auto outfile = make_shared(); 45 | outfiles.push_back(outfile); 46 | 47 | char filename[512]; 48 | 49 | if (extensions[i].compare("") == 0) extensions[i] = "obj"; 50 | 51 | if (types[i] == Brenderable::Truncate) { 52 | // if object has not been given name 53 | if (names[i].compare("") == 0) { 54 | sprintf(filename, "%s/%06d_Object%d.%s", EXPORT_DIR_.c_str(), frame_, objNum, extensions[i].c_str()); 55 | } 56 | // if object has been given specific name 57 | else { 58 | sprintf(filename, "%s/%06d_%s.%s", EXPORT_DIR_.c_str(), frame_, names[i].c_str(), extensions[i].c_str()); 59 | } 60 | 61 | // open file 62 | outfile->open(filename, ofstream::out | ofstream::trunc); 63 | 64 | // frame string 65 | char framestring[50]; 66 | sprintf(framestring, "# frame %06d \n", frame_); 67 | *outfile << framestring; 68 | // frame time 69 | char timeval[50]; 70 | sprintf(timeval, "# time %f \n", time); 71 | *outfile << timeval; 72 | // obj name 73 | // if object has not been given name 74 | if (names[i].compare("") == 0) { 75 | *outfile << "# name Object" + to_string(objNum) + " \n"; 76 | } 77 | // if object has been given specific name 78 | else { 79 | *outfile << "# name " + names[i] + " \n"; 80 | } 81 | } 82 | else if (types[i] == Brenderable::Append || types[i] == Brenderable::ResetAppend) { 83 | // if object has not been given name 84 | if (names[i].compare("") == 0) { 85 | sprintf(filename, "%s/Object%d.%s", EXPORT_DIR_.c_str(), objNum, extensions[i].c_str()); 86 | } 87 | // if object has been given specific name 88 | else { 89 | sprintf(filename, "%s/%s.%s", EXPORT_DIR_.c_str(), names[i].c_str(), extensions[i].c_str()); 90 | } 91 | 92 | if (types[i] == Brenderable::Append) { 93 | outfile->open(filename, ofstream::out | ofstream::app); 94 | } 95 | else { 96 | outfile->open(filename, ofstream::out | ofstream::trunc); 97 | } 98 | } 99 | } 100 | // Write to files 101 | brenderable->exportBrender(outfiles, frame_, time); 102 | // Close files 103 | for (int i = 0; i < brenderable->getBrenderCount(); ++i) { 104 | outfiles[i]->close(); 105 | } 106 | objNum++; 107 | } 108 | //Only time frame should be changed/modified 109 | frame_++; 110 | } 111 | 112 | void BrenderManager::add(shared_ptr brenderable) 113 | { 114 | brenderables_.push_back(brenderable); 115 | } 116 | 117 | void BrenderManager::setExportDir(string export_dir) 118 | { 119 | EXPORT_DIR_ = export_dir; 120 | } 121 | 122 | -------------------------------------------------------------------------------- /matlab/+redmax/Deformable.m: -------------------------------------------------------------------------------- 1 | classdef (Abstract) Deformable < handle 2 | 3 | %% 4 | properties 5 | name 6 | color 7 | damping 8 | next 9 | end 10 | 11 | %% 12 | methods 13 | %% 14 | function this = Deformable() 15 | global countD countCM CM; 16 | this.name = ['deformable',num2str(countD)]; 17 | this.color = CM(mod(countCM-1,size(CM,1))+1,:); 18 | this.damping = 0; 19 | countD = countD + 1; 20 | countCM = countCM + 1; 21 | end 22 | 23 | %% 24 | function setDamping(this,damping) 25 | % Sets viscous damping 26 | this.damping = damping; 27 | end 28 | 29 | %% 30 | function initGeometry(this) 31 | this.initGeometry_(); 32 | if ~isempty(this.next) 33 | this.next.initGeometry(); 34 | end 35 | end 36 | 37 | %% 38 | function countDofs(this) 39 | this.countDofs_(); 40 | if ~isempty(this.next) 41 | this.next.countDofs(); 42 | end 43 | end 44 | 45 | %% 46 | function y = gatherDofs(this,y) 47 | y = this.gatherDofs_(y); 48 | if ~isempty(this.next) 49 | y = this.next.gatherDofs(y); 50 | end 51 | end 52 | 53 | %% 54 | function ydot = gatherDDofs(this,ydot) 55 | ydot = this.gatherDDofs(ydot); 56 | if ~isempty(this.next) 57 | ydot = this.next.gatherDDofs(ydot); 58 | end 59 | end 60 | 61 | %% 62 | function scatterDofs(this,y) 63 | this.scatterDofs_(y); 64 | if ~isempty(this.next) 65 | this.next.scatterDofs(y); 66 | end 67 | end 68 | 69 | %% 70 | function scatterDDofs(this,ydot) 71 | this.scatterDDofs_(ydot); 72 | if ~isempty(this.next) 73 | this.next.scatterDDofs(ydot); 74 | end 75 | end 76 | 77 | %% 78 | function [J,Jdot] = computeJacobian(this,J,Jdot) 79 | [J,Jdot] = this.computeJacobian_(J,Jdot); 80 | if ~isempty(this.next) 81 | [J,Jdot] = this.next.computeJacobian(J,Jdot); 82 | end 83 | end 84 | 85 | %% 86 | function [M,f] = computeMassGrav(this,grav,M,f) 87 | [M,f] = this.computeMassGrav_(grav,M,f); 88 | if ~isempty(this.next) 89 | [M,f] = this.next.computeMassGrav(grav,M,f); 90 | end 91 | end 92 | 93 | %% 94 | function [f,D] = computeForceDamping(this,f,D) 95 | [f,D] = this.computeForceDamping_(f,D); 96 | if ~isempty(this.next) 97 | [f,D] = this.next.computeForceDamping(f,D); 98 | end 99 | end 100 | 101 | %% 102 | function [T,V] = computeEnergies(this,grav,T,V) 103 | [T,V] = this.computeEnergies_(grav,T,V); 104 | if ~isempty(this.next) 105 | [T,V] = this.next.computeEnergies(grav,T,V); 106 | end 107 | end 108 | 109 | %% 110 | function draw(this) 111 | this.draw_(); 112 | if ~isempty(this.next) 113 | this.next.draw(); 114 | end 115 | end 116 | end 117 | 118 | %% 119 | methods (Access = protected) 120 | %% 121 | function initGeometry_(this) %#ok 122 | end 123 | 124 | %% 125 | function countDofs_(this) %#ok 126 | end 127 | 128 | %% 129 | function y = gatherDofs_(this,y) %#ok 130 | end 131 | 132 | %% 133 | function ydot = gatherDDofs_(this,ydot) %#ok 134 | end 135 | 136 | %% 137 | function scatterDofs_(this,y) %#ok 138 | end 139 | 140 | %% 141 | function scatterDDofs_(this,ydot) %#ok 142 | end 143 | 144 | %% 145 | function [J,Jdot] = computeJacobian_(this,J,Jdot) %#ok 146 | end 147 | 148 | %% 149 | function [M,f] = computeMassGrav_(this,grav,M,f) %#ok 150 | end 151 | 152 | %% 153 | function [f,D] = computeForceDamping_(this,f,D) %#ok 154 | end 155 | 156 | %% 157 | function [T,V] = computeEnergies_(this,grav,T,V) %#ok 158 | end 159 | 160 | %% 161 | function draw_(this) %#ok 162 | end 163 | end 164 | end 165 | 166 | -------------------------------------------------------------------------------- /c++/PCG/src/online/GLSL.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Many useful helper functions for GLSL shaders - gleaned from various sources including orange book 3 | // Created by zwood on 2/21/10. 4 | // Modified by sueda 10/15/15. 5 | // 6 | 7 | #include "GLSL.h" 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace std; 14 | 15 | namespace GLSL { 16 | 17 | const char * errorString(GLenum err) 18 | { 19 | switch(err) { 20 | case GL_NO_ERROR: 21 | return "No error"; 22 | case GL_INVALID_ENUM: 23 | return "Invalid enum"; 24 | case GL_INVALID_VALUE: 25 | return "Invalid value"; 26 | case GL_INVALID_OPERATION: 27 | return "Invalid operation"; 28 | case GL_STACK_OVERFLOW: 29 | return "Stack overflow"; 30 | case GL_STACK_UNDERFLOW: 31 | return "Stack underflow"; 32 | case GL_OUT_OF_MEMORY: 33 | return "Out of memory"; 34 | default: 35 | return "No error"; 36 | } 37 | } 38 | 39 | void checkVersion() 40 | { 41 | int major, minor; 42 | major = minor = 0; 43 | const char *verstr = (const char *)glGetString(GL_VERSION); 44 | 45 | if((verstr == NULL) || (sscanf(verstr, "%d.%d", &major, &minor) != 2)) { 46 | printf("Invalid GL_VERSION format %d.%d\n", major, minor); 47 | } 48 | if(major < 2) { 49 | printf("This shader example will not work due to the installed Opengl version, which is %d.%d.\n", major, minor); 50 | exit(0); 51 | } 52 | } 53 | 54 | void checkError(const char *str) 55 | { 56 | GLenum glErr = glGetError(); 57 | if(glErr != GL_NO_ERROR) { 58 | if(str) { 59 | printf("%s: ", str); 60 | } 61 | printf("GL_ERROR = %s.\n", errorString(glErr)); 62 | assert(false); 63 | } 64 | } 65 | 66 | void printShaderInfoLog(GLuint shader) 67 | { 68 | GLint infologLength = 0; 69 | GLint charsWritten = 0; 70 | GLchar *infoLog = 0; 71 | 72 | checkError(GET_FILE_LINE); 73 | glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &infologLength); 74 | checkError(GET_FILE_LINE); 75 | 76 | if(infologLength > 0) { 77 | infoLog = (GLchar *)malloc(infologLength); 78 | if(infoLog == NULL) { 79 | puts("ERROR: Could not allocate InfoLog buffer"); 80 | exit(1); 81 | } 82 | glGetShaderInfoLog(shader, infologLength, &charsWritten, infoLog); 83 | checkError(GET_FILE_LINE); 84 | printf("Shader InfoLog:\n%s\n\n", infoLog); 85 | free(infoLog); 86 | } 87 | } 88 | 89 | void printProgramInfoLog(GLuint program) 90 | { 91 | GLint infologLength = 0; 92 | GLint charsWritten = 0; 93 | GLchar *infoLog = 0; 94 | 95 | checkError(GET_FILE_LINE); 96 | glGetProgramiv(program, GL_INFO_LOG_LENGTH, &infologLength); 97 | checkError(GET_FILE_LINE); 98 | 99 | if(infologLength > 0) { 100 | infoLog = (GLchar *)malloc(infologLength); 101 | if(infoLog == NULL) { 102 | puts("ERROR: Could not allocate InfoLog buffer"); 103 | exit(1); 104 | } 105 | glGetProgramInfoLog(program, infologLength, &charsWritten, infoLog); 106 | checkError(GET_FILE_LINE); 107 | printf("Program InfoLog:\n%s\n\n", infoLog); 108 | free(infoLog); 109 | } 110 | } 111 | 112 | char *textFileRead(const char *fn) 113 | { 114 | FILE *fp; 115 | char *content = NULL; 116 | int count = 0; 117 | if(fn != NULL) { 118 | fp = fopen(fn,"rt"); 119 | if(fp != NULL) { 120 | fseek(fp, 0, SEEK_END); 121 | count = (int)ftell(fp); 122 | rewind(fp); 123 | if(count > 0) { 124 | content = (char *)malloc(sizeof(char) * (count+1)); 125 | count = (int)fread(content,sizeof(char),count,fp); 126 | content[count] = '\0'; 127 | } 128 | fclose(fp); 129 | } else { 130 | printf("error loading %s\n", fn); 131 | } 132 | } 133 | return content; 134 | } 135 | 136 | int textFileWrite(const char *fn, const char *s) 137 | { 138 | FILE *fp; 139 | int status = 0; 140 | if(fn != NULL) { 141 | fp = fopen(fn,"w"); 142 | if(fp != NULL) { 143 | if(fwrite(s,sizeof(char),strlen(s),fp) == strlen(s)) { 144 | status = 1; 145 | } 146 | fclose(fp); 147 | } 148 | } 149 | return(status); 150 | } 151 | 152 | } 153 | -------------------------------------------------------------------------------- /c++/PCG/src/RigidBodyCreator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "RigidBodyUtility.h" 6 | #include "Block.h" 7 | 8 | struct Joint; 9 | struct LinkageSystem; 10 | struct State; 11 | struct StateSolve; 12 | 13 | class RigidBodyCreator 14 | { 15 | public: 16 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 17 | 18 | RigidBodyCreator(double t): 19 | t(t) 20 | {}; 21 | ~RigidBodyCreator() {}; 22 | 23 | enum dynamicType { chain, tree }; 24 | 25 | void loadTree( 26 | int n, 27 | simType s, 28 | std::unique_ptr& LS, 29 | std::unique_ptr& S, 30 | std::unique_ptr& SS); 31 | void loadBridge( 32 | int nbridge, 33 | int ntower, 34 | simType s, 35 | std::unique_ptr& LS, 36 | std::unique_ptr& S, 37 | std::unique_ptr& SS); 38 | void loadSimpleBridge( 39 | int nbridge, 40 | simType s, 41 | std::unique_ptr& LS, 42 | std::unique_ptr& S, 43 | std::unique_ptr& SS); 44 | void loadSimpleTree( 45 | int n, 46 | simType s, 47 | std::unique_ptr& LS, 48 | std::unique_ptr& S, 49 | std::unique_ptr& SS); 50 | void loadUmbrella( 51 | int n, 52 | simType s, 53 | std::unique_ptr& LS, 54 | std::unique_ptr& S, 55 | std::unique_ptr& SS); 56 | void loadTest( 57 | int n, 58 | simType s, 59 | std::unique_ptr& LS, 60 | std::unique_ptr& S, 61 | std::unique_ptr& SS); 62 | void loadChain( 63 | int n, 64 | simType s, 65 | std::unique_ptr& LS, 66 | std::unique_ptr& S, 67 | std::unique_ptr& SS); 68 | void loadLinkagesfromFile( 69 | std::unique_ptr& LS, 70 | std::unique_ptr& S, 71 | std::unique_ptr& SS, 72 | const bool disconnect); 73 | void loadPhysicsfromFile( 74 | std::unique_ptr& LS, 75 | std::unique_ptr& S); 76 | 77 | void disconnectInertial( 78 | std::unique_ptr& LS, 79 | std::unique_ptr& S, 80 | std::string name, 81 | Eigen::Vector3d &p, 82 | Eigen::Matrix3d &basis, 83 | double density = 0.0, 84 | double x = 0.0, 85 | double y = 0.0, 86 | double z = 0.0); 87 | 88 | void setResourceDir(const std::string &RESOURCE_DIR) { this->RESOURCE_DIR = RESOURCE_DIR; }; 89 | void setUserSource(const std::string &FILENAME) { this->USER_SOURCE = FILENAME; }; 90 | void useSourceLinks() { activeLinks = USER_SOURCE; }; 91 | void useSnappedLinks() { activeLinks = SNAPPED_LINKAGES; }; 92 | void useSourceSplines() { activeSplines = USER_SOURCE; }; 93 | void useUserSpline() { activeSplines = USER_SPLINE; }; 94 | void useFitSpline() { activeSplines = FIT_SPLINE; }; 95 | void useResultSpline() { activeSplines = SPLINE_RESULT; }; 96 | 97 | std::string getResourceDir() { return RESOURCE_DIR; }; 98 | 99 | void printLinkages( 100 | const std::unique_ptr& LS, 101 | const std::unique_ptr& S); 102 | private: 103 | double &t; // reference 'global' sim time - from rigid body main 104 | 105 | const std::string SNAPPED_LINKAGES = "linkages_auto_save.txt"; 106 | const std::string USER_SPLINE = "splines_auto_save.txt"; 107 | const std::string FIT_SPLINE = "fit_splines_auto_save.txt"; 108 | const std::string SPLINE_RESULT = "result_spline_auto_save.txt"; 109 | std::string RESOURCE_DIR; 110 | std::string USER_SOURCE; 111 | std::string activeLinks; 112 | std::string activeSplines; 113 | 114 | void addDisplayBlock( 115 | std::unique_ptr & LS, 116 | std::unique_ptr& S, 117 | std::string name, 118 | double density, 119 | double damping, 120 | double xwidth, 121 | double ywidth, 122 | double zwidth, 123 | std::string parent_name, 124 | std::shared_ptr &j, 125 | Eigen::Vector3d &pos, 126 | Eigen::Matrix3d &basis, 127 | Eigen::Vector3d &parent_jpos = emptyVector3d, 128 | Block::SType stype = Block::SType::Cuboid 129 | ); 130 | }; 131 | -------------------------------------------------------------------------------- /matlab/+redmax/JointPrismatic.m: -------------------------------------------------------------------------------- 1 | classdef JointPrismatic < redmax.Joint 2 | % https://github.com/junggon/gear/blob/master/src/gjoint_prismatic.cpp 3 | 4 | %% 5 | properties 6 | axis 7 | sides 8 | end 9 | 10 | %% 11 | methods 12 | %% 13 | function this = JointPrismatic(parent,body,axis) 14 | this = this@redmax.Joint(parent,body,1); 15 | this.axis = axis; 16 | this.sides = [0.5 0.5 0.5]; 17 | end 18 | 19 | %% 20 | function setGeometry(this,sides) 21 | this.sides = sides; 22 | end 23 | end 24 | 25 | %% 26 | methods (Access = protected) 27 | %% 28 | function update_(this) 29 | this.Q = eye(4); 30 | this.Q(1:3,4) = this.axis*this.q(1); 31 | this.S(4:6,1) = this.axis; 32 | end 33 | 34 | %% 35 | function generateContacts_(this) 36 | % The contact points will be generated in the 'A' frame, which 37 | % is a frame defined with its Z-axis as the direction of 38 | % translation of this joint. 39 | S = eye(4); 40 | S(1:3,1:3) = diag(this.sides); 41 | E_ja = eye(4); 42 | z = [0 0 1]'; 43 | angle = acos(max(-1.0, min(this.axis'*z, 1.0))); 44 | E_ja(1:3,1:3) = se3.aaToMat(cross(this.axis,z),angle); 45 | E_ia = this.body.E_ij * S * E_ja; 46 | x0 = -0.5; 47 | x1 = 0.5; 48 | y0 = -0.5; 49 | y1 = 0.5; 50 | for kz = 1 : 2 51 | z = (kz-1.5); 52 | tan_a = [0 0 1]'; 53 | tan_i = E_ia(1:3,:)*[tan_a;0]; 54 | tan_i = tan_i/norm(tan_i); 55 | for kx = 1 : 2 56 | for ky = 1 : 2 57 | if kx == 1 && ky == 1 58 | pos_a = [x0 y0 z]'; 59 | nor1_a = [1 0 0]'; 60 | nor2_a = [0 1 0]'; 61 | elseif kx == 1 && ky == 2 62 | pos_a = [x0 y1 z]'; 63 | nor1_a = [1 0 0]'; 64 | nor2_a = [0 -1 0]'; 65 | elseif kx == 2 && ky == 1 66 | pos_a = [x1 y0 z]'; 67 | nor1_a = [-1 0 0]'; 68 | nor2_a = [0 1 0]'; 69 | else 70 | pos_a = [x1 y1 z]'; 71 | nor1_a = [-1 0 0]'; 72 | nor2_a = [0 -1 0]'; 73 | end 74 | pos_i = E_ia(1:3,:)*[pos_a;1]; 75 | nor1_i = E_ia(1:3,:)*[nor1_a;0]; 76 | nor2_i = E_ia(1:3,:)*[nor2_a;0]; 77 | nor1_i = nor1_i/norm(nor1_i); 78 | nor2_i = nor2_i/norm(nor2_i); 79 | this.contacts{end+1}.pos_i = pos_i; 80 | this.contacts{end}.nor_i = nor1_i; 81 | this.contacts{end}.tan_i = tan_i; 82 | this.contacts{end+1}.pos_i = pos_i; 83 | this.contacts{end}.nor_i = nor2_i; 84 | this.contacts{end}.tan_i = tan_i; 85 | end 86 | end 87 | end 88 | % Index for tangent matrix 89 | nt = redmax.Scene.countT(); 90 | t = length(this.contacts); 91 | this.idxT = nt + (1:t); 92 | nt = nt + t; 93 | redmax.Scene.countT(nt); 94 | end 95 | 96 | %% 97 | function T = computeTangentMatrix_(this,T) 98 | for i = 1 : length(this.contacts) 99 | contact = this.contacts{i}; 100 | tan_i = contact.tan_i; 101 | pos_i = contact.pos_i; 102 | T(this.idxT(i),this.body.idxM) = tan_i'*se3.Gamma(pos_i); 103 | if ~isempty(this.parent) 104 | E_pi = this.parent.body.E_iw*this.body.E_wi; 105 | tan_p = E_pi(1:3,:)*[tan_i;0]; 106 | pos_p = E_pi(1:3,:)*[pos_i;1]; 107 | T(this.idxT(i),this.parent.body.idxM) = -tan_p'*se3.Gamma(pos_p); 108 | end 109 | end 110 | end 111 | 112 | %% 113 | function [bl,bu,idx] = computeFrictionLimits_(this,mu,SPathresh,bl,bu,idx) 114 | % Combine normal and binormal, since they share the same 115 | % tangent. 116 | for i0 = 1 : 2 : length(this.contacts) 117 | i1 = i0 + 1; 118 | a0 = abs(this.contacts{i0}.a); 119 | a1 = abs(this.contacts{i1}.a); 120 | a = a0 + a1; 121 | bl(this.idxT(i0)) = -mu*a; 122 | bu(this.idxT(i0)) = mu*a; 123 | if a > SPathresh 124 | idx(end+1) = this.idxT(i0); %#ok 125 | end 126 | end 127 | end 128 | 129 | %% 130 | function draw_(this) 131 | E_wj = this.body.E_wi*this.body.E_ij; 132 | [F,V] = se3.patchCuboid(E_wj,this.sides); 133 | patch('Faces',F,'Vertices',V,'FaceColor',this.body.color); 134 | end 135 | end 136 | end 137 | -------------------------------------------------------------------------------- /matlab/+redmax/ConstraintFloor.m: -------------------------------------------------------------------------------- 1 | classdef ConstraintFloor < redmax.Constraint 2 | % ConstraintFloor Inequality contact with the floor. 3 | % Assumptions for simplicity: 4 | % - The body is a sphere. 5 | % - There is one contact point per body. 6 | 7 | %% 8 | properties 9 | body % Body to apply the constraint on 10 | E % The "floor" frame, with Z-up. 11 | end 12 | 13 | %% 14 | methods 15 | %% 16 | function this = ConstraintFloor(body) 17 | this = this@redmax.Constraint(0,0,1,0); 18 | this.name = [body.name,'-FLOOR']; 19 | this.body = body; 20 | this.E = eye(4); 21 | end 22 | 23 | %% 24 | function setTransform(this,E) 25 | this.E = E; 26 | end 27 | end 28 | 29 | %% 30 | methods (Access = protected) 31 | %% 32 | function [Cm,Cmdot,cm,cmdot,cmddot] = computeJacIneqM_(this,Cm,Cmdot,cm,cmdot,cmddot) 33 | rows = this.idxIM; 34 | cols = this.body.idxM; 35 | this.idxQ = cols; 36 | % Assuming a sphere 37 | r = this.body.radius; 38 | x_w = this.body.E_wi(:,4); % body center in world frame 39 | x_f = this.E\x_w; % transformed to "floor" frame 40 | z = x_f(3); 41 | if z < r 42 | % Contact point is x_f, with radius subtracted from Z 43 | x = x_f; 44 | x(3) = x(3) - r; 45 | % Transform to world then to body 46 | x = this.body.E_wi\(this.E*x); 47 | G = se3.Gamma(x(1:3)); 48 | nor = this.E(1:3,3); 49 | R = this.body.E_wi(1:3,1:3); 50 | Cm(rows,cols) = -nor'*R*G; 51 | cm(rows) = r - z; 52 | this.activeM = true; 53 | else 54 | this.activeM = false; 55 | end 56 | end 57 | 58 | %% 59 | function generateContactsCollision_(this) 60 | if ~this.activeM 61 | return; 62 | end 63 | % Assuming a sphere 64 | r = this.body.radius; 65 | E_wi = this.body.E_wi; 66 | R_wi = E_wi(1:3,1:3); 67 | E_wf = this.E; 68 | x_w = E_wi(:,4); % body center in world frame 69 | x_f = E_wf\x_w; % transformed to "floor" frame 70 | % Contact point is x_f, with radius subtracted from Z 71 | x = x_f; 72 | x(3) = x(3) - r; 73 | % Transform to world then to body 74 | x = E_wi\(E_wf*x); 75 | % Normal 76 | nor_w = E_wf(1:3,3); 77 | nor_a = R_wi'*nor_w; 78 | % Tangents (use X and Y from the floor) 79 | tan_w = E_wf(1:3,1:2); 80 | tan_a = R_wi'*tan_w; 81 | this.contacts{1}.pos_a = x(1:3); 82 | this.contacts{1}.nor_a = nor_a; 83 | this.contacts{1}.tan_a = tan_a; % two tangents 84 | % Index for tangent matrix 85 | nt = redmax.Scene.countT(); 86 | this.idxT = nt + (1:2); 87 | nt = nt + 2; 88 | redmax.Scene.countT(nt); 89 | end 90 | 91 | %% 92 | function scatterForceIneqM_(this,Cmt,lm) %#ok<*INUSL> 93 | if ~this.activeM 94 | return; 95 | end 96 | this.contacts{1}.a = lm(this.idxIM); 97 | end 98 | 99 | %% 100 | function computeContactMultiplier_(this,h,SPreg) %#ok 101 | % Since the Lagrange multiplier passed in through the function 102 | % scatterForceIneqM_() was divided by h, we multiply by h here. 103 | % This is a bit of a hack! 104 | if ~this.activeM 105 | return; 106 | end 107 | this.contacts{1}.a = this.contacts{1}.a*h; 108 | end 109 | 110 | %% 111 | function T = computeTangentMatrix_(this,T) 112 | if ~this.activeM 113 | return; 114 | end 115 | contact = this.contacts{1}; 116 | tan_a = contact.tan_a; % 3x2 matrix 117 | pos_a = contact.pos_a; 118 | T(this.idxT,this.body.idxM) = tan_a'*se3.Gamma(pos_a); 119 | end 120 | 121 | %% 122 | function [bl,bu,idx] = computeFrictionLimits_(this,mu,SPathresh,bl,bu,idx) 123 | if ~this.activeM 124 | return; 125 | end 126 | a = this.contacts{1}.a; 127 | % Use external mu 128 | bl(this.idxT(1)) = -mu(2)*a; 129 | bu(this.idxT(1)) = mu(2)*a; 130 | bl(this.idxT(2)) = -mu(2)*a; 131 | bu(this.idxT(2)) = mu(2)*a; 132 | if a > SPathresh 133 | idx(end+(1:2)) = this.idxT; 134 | end 135 | end 136 | 137 | %% 138 | function draw_(this) 139 | se3.drawAxis(this.E); 140 | s = 10; 141 | v = this.E(1:3,:)*[-s -s 0 1; s -s 0 1; s s 0 1; -s s 0 1]'; 142 | f = [1 2 3 4]; 143 | patch('Faces',f,'Vertices',v','FaceColor',[0.5 0.5 0.5]) 144 | end 145 | end 146 | end 147 | -------------------------------------------------------------------------------- /c++/PCG/cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | # 13 | # and the following imported target: 14 | # 15 | # Eigen3::Eigen - The header-only Eigen library 16 | # 17 | # This module reads hints about search locations from 18 | # the following environment variables: 19 | # 20 | # EIGEN3_ROOT 21 | # EIGEN3_ROOT_DIR 22 | 23 | # Copyright (c) 2006, 2007 Montel Laurent, 24 | # Copyright (c) 2008, 2009 Gael Guennebaud, 25 | # Copyright (c) 2009 Benoit Jacob 26 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 27 | 28 | SET(EIGEN3_INCLUDE_DIR ${EIGEN3_ROOT_DIR}) 29 | IF(NOT EIGEN3_INCLUDE_DIR) 30 | SET(ERR_MSG "Please point the variable EIGEN3_ROOT_DIR to the root directory of your EIGEN installation.") 31 | MESSAGE(FATAL_ERROR ${ERR_MSG}) 32 | ENDIF() 33 | 34 | if(NOT Eigen3_FIND_VERSION) 35 | if(NOT Eigen3_FIND_VERSION_MAJOR) 36 | set(Eigen3_FIND_VERSION_MAJOR 2) 37 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 38 | if(NOT Eigen3_FIND_VERSION_MINOR) 39 | set(Eigen3_FIND_VERSION_MINOR 91) 40 | endif(NOT Eigen3_FIND_VERSION_MINOR) 41 | if(NOT Eigen3_FIND_VERSION_PATCH) 42 | set(Eigen3_FIND_VERSION_PATCH 0) 43 | endif(NOT Eigen3_FIND_VERSION_PATCH) 44 | 45 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 46 | endif(NOT Eigen3_FIND_VERSION) 47 | 48 | macro(_eigen3_check_version) 49 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 50 | 51 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 52 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 53 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 54 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 55 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 56 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 57 | 58 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 59 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 60 | set(EIGEN3_VERSION_OK FALSE) 61 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 62 | set(EIGEN3_VERSION_OK TRUE) 63 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 64 | 65 | if(NOT EIGEN3_VERSION_OK) 66 | 67 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 68 | "but at least version ${Eigen3_FIND_VERSION} is required") 69 | endif(NOT EIGEN3_VERSION_OK) 70 | endmacro(_eigen3_check_version) 71 | 72 | if (EIGEN3_INCLUDE_DIR) 73 | 74 | # in cache already 75 | _eigen3_check_version() 76 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 77 | set(Eigen3_FOUND ${EIGEN3_VERSION_OK}) 78 | 79 | else (EIGEN3_INCLUDE_DIR) 80 | 81 | # search first if an Eigen3Config.cmake is available in the system, 82 | # if successful this would set EIGEN3_INCLUDE_DIR and the rest of 83 | # the script will work as usual 84 | find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET) 85 | 86 | if(NOT EIGEN3_INCLUDE_DIR) 87 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 88 | HINTS 89 | ENV EIGEN3_ROOT 90 | ENV EIGEN3_ROOT_DIR 91 | PATHS 92 | ${EIGEN3_ROOT_DIR}/ 93 | ${CMAKE_INSTALL_PREFIX}/include/ 94 | ${KDE4_INCLUDE_DIR}/ 95 | PATH_SUFFIXES eigen3 eigen 96 | ) 97 | endif(NOT EIGEN3_INCLUDE_DIR) 98 | 99 | if(EIGEN3_INCLUDE_DIR) 100 | _eigen3_check_version() 101 | endif(EIGEN3_INCLUDE_DIR) 102 | 103 | include(FindPackageHandleStandardArgs) 104 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 105 | 106 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 107 | 108 | endif(EIGEN3_INCLUDE_DIR) 109 | 110 | if(EIGEN3_FOUND AND NOT TARGET Eigen3::Eigen) 111 | add_library(Eigen3::Eigen INTERFACE IMPORTED) 112 | set_target_properties(Eigen3::Eigen PROPERTIES 113 | INTERFACE_INCLUDE_DIRECTORIES "${EIGEN3_INCLUDE_DIR}") 114 | endif() 115 | -------------------------------------------------------------------------------- /matlab-simple/testRedMaxScenes.m: -------------------------------------------------------------------------------- 1 | function scene = testRedMaxScenes(sceneID) 2 | % Creates test scenes 3 | 4 | scene = redmax.Scene(); 5 | 6 | switch(sceneID) 7 | case -1 8 | scene.name = 'Simpler serial chain'; 9 | sides = [10 1 1]; 10 | nbodies = 2; 11 | density = 1.0; 12 | scene.waxis = nbodies*5*[-1 1 -1 1 -2 0]; 13 | for i = 1 : nbodies 14 | scene.bodies{i} = redmax.BodyCuboid(density,sides); %#ok<*SAGROW> 15 | if i == 1 16 | scene.joints{i} = redmax.JointRevolute([],scene.bodies{i},[0 1 0]'); 17 | scene.joints{i}.setJointTransform(eye(4)); 18 | scene.joints{i}.q(1) = pi/2; 19 | else 20 | scene.joints{i} = redmax.JointRevolute(scene.joints{i-1},scene.bodies{i},[0 1 0]'); 21 | scene.joints{i}.setJointTransform([eye(3),[10 0 0]'; 0 0 0 1]); 22 | scene.joints{i}.q(1) = pi/4; 23 | end 24 | scene.bodies{i}.setBodyTransform([eye(3),[5 0 0]'; 0 0 0 1]); 25 | % Body damping 26 | scene.bodies{i}.setDamping(1e0); 27 | % Joint stiffness and damping 28 | scene.joints{i}.setStiffness(1e6); 29 | scene.joints{i}.setDamping(1e0); 30 | end 31 | case 0 32 | scene.name = 'Simple serial chain'; 33 | sides = [10 1 1]; 34 | nbodies = 5; 35 | density = 1.0; 36 | scene.waxis = nbodies*5*[-1 1 -0.1 0.1 -2 0]; 37 | for i = 1 : nbodies 38 | scene.bodies{i} = redmax.BodyCuboid(density,sides); %#ok<*SAGROW> 39 | if i == 1 40 | scene.joints{i} = redmax.JointRevolute([],scene.bodies{i},[0 1 0]'); 41 | scene.joints{i}.setJointTransform(eye(4)); 42 | else 43 | if mod(i,2) == 1 44 | scene.joints{i} = redmax.JointRevolute(scene.joints{i-1},scene.bodies{i},[0 1 0]'); 45 | else 46 | scene.joints{i} = redmax.JointFixed(scene.joints{i-1},scene.bodies{i}); 47 | end 48 | %scene.joints{i} = redmax.JointRevolute(scene.joints{i-1},scene.bodies{i},[0 1 0]'); 49 | scene.joints{i}.setJointTransform([eye(3),[10 0 0]'; 0 0 0 1]); 50 | end 51 | scene.bodies{i}.setBodyTransform([eye(3),[5 0 0]'; 0 0 0 1]); 52 | if mod(i,2) == 1 53 | scene.joints{i}.q(1) = pi/4; 54 | else 55 | scene.joints{i}.q(1) = 0; 56 | end 57 | end 58 | case 1 59 | scene.name = 'Different revolute axes'; 60 | sides = [10 1 1]; 61 | density = 1.0; 62 | scene.waxis = [-20 20 -20 20 -20 5]; 63 | scene.bodies{1} = redmax.BodyCuboid(density,sides); 64 | scene.bodies{2} = redmax.BodyCuboid(density,sides); 65 | scene.bodies{3} = redmax.BodyCuboid(density,sides); 66 | scene.joints{1} = redmax.JointRevolute([], scene.bodies{1},[0 0 1]'); 67 | scene.joints{2} = redmax.JointRevolute(scene.joints{1},scene.bodies{2},[0 1 0]'); 68 | scene.joints{3} = redmax.JointRevolute(scene.joints{2},scene.bodies{3},[0 0 1]'); 69 | scene.bodies{1}.setBodyTransform([eye(3),[5 0 0]'; 0 0 0 1]); 70 | scene.bodies{2}.setBodyTransform([eye(3),[5 0 0]'; 0 0 0 1]); 71 | scene.bodies{3}.setBodyTransform([eye(3),[5 0 0]'; 0 0 0 1]); 72 | scene.joints{1}.setJointTransform(eye(4)); 73 | scene.joints{2}.setJointTransform([eye(3),[10 0 0]'; 0 0 0 1]); 74 | scene.joints{3}.setJointTransform([eye(3),[10 0 0]'; 0 0 0 1]); 75 | scene.joints{1}.q(1) = 0; 76 | scene.joints{2}.q(1) = pi/2; 77 | scene.joints{3}.q(1) = pi/2; 78 | case 2 79 | scene.name = 'Branching'; 80 | % X in YZ plane 81 | % | 82 | % X---Z---Y joint axes shown 83 | % | | 84 | % | | 85 | density = 1.0; 86 | scene.waxis = [-15 15 -15 15 -10 20]; 87 | scene.bodies{1} = redmax.BodyCuboid(density,[1 1 10]); 88 | scene.bodies{2} = redmax.BodyCuboid(density,[1 20 1]); 89 | scene.bodies{3} = redmax.BodyCuboid(density,[1 1 10]); 90 | scene.bodies{4} = redmax.BodyCuboid(density,[1 1 10]); 91 | scene.joints{1} = redmax.JointRevolute([], scene.bodies{1},[1 0 0]'); 92 | scene.joints{2} = redmax.JointRevolute(scene.joints{1},scene.bodies{2},[0 0 1]'); 93 | scene.joints{3} = redmax.JointRevolute(scene.joints{2},scene.bodies{3},[1 0 0]'); 94 | scene.joints{4} = redmax.JointRevolute(scene.joints{2},scene.bodies{4},[0 1 0]'); 95 | scene.bodies{1}.setBodyTransform([eye(3),[0 0 -5]'; 0 0 0 1]); 96 | scene.bodies{2}.setBodyTransform([eye(3),[0 0 0]'; 0 0 0 1]); 97 | scene.bodies{3}.setBodyTransform([eye(3),[0 0 -5]'; 0 0 0 1]); 98 | scene.bodies{4}.setBodyTransform([eye(3),[0 0 -5]'; 0 0 0 1]); 99 | scene.joints{1}.setJointTransform([eye(3),[0 0 15]'; 0 0 0 1]); 100 | scene.joints{2}.setJointTransform([eye(3),[0 0 -10]'; 0 0 0 1]); 101 | scene.joints{3}.setJointTransform([eye(3),[0 -10 0]'; 0 0 0 1]); 102 | scene.joints{4}.setJointTransform([eye(3),[0 10 0]'; 0 0 0 1]); 103 | scene.joints{1}.q(1) = 0; 104 | scene.joints{2}.q(1) = 0; 105 | scene.joints{3}.q(1) = pi/4; 106 | scene.joints{4}.q(1) = pi/4; 107 | 108 | end 109 | 110 | end 111 | -------------------------------------------------------------------------------- /matlab/+redmax/SpringPointPoint.m: -------------------------------------------------------------------------------- 1 | classdef SpringPointPoint < redmax.Spring 2 | %SpringPointPoint A linear force between two points 3 | 4 | %% 5 | properties 6 | stiffness 7 | body1 % Body to apply the wrench to (null implies world) 8 | body2 % Body to apply the wrench to (null implies world) 9 | x_1 % Application point in local coords 10 | x_2 % Application point in local coords 11 | end 12 | 13 | methods 14 | function this = SpringPointPoint(body1,x_1,body2,x_2) 15 | this = this@redmax.Spring(); 16 | this.body1 = body1; 17 | this.body2 = body2; 18 | this.x_1 = x_1; 19 | this.x_2 = x_2; 20 | this.stiffness = 0; 21 | end 22 | 23 | %% 24 | function setStiffness(this,stiffness) 25 | this.stiffness = stiffness; 26 | end 27 | end 28 | 29 | %% 30 | methods (Access = protected) 31 | %% 32 | function [f,K,D] = computeForceStiffnessDamping_(this,f,K,D) 33 | [f_,K_] = this.computeFK(); 34 | idxM1 = []; 35 | idxM2 = []; 36 | if ~isempty(this.body1) 37 | idxM1 = this.body1.idxM; 38 | f1 = f_(1:6); 39 | K11 = K_(1:6,1:6); 40 | f(idxM1) = f(idxM1) + f1; 41 | K(idxM1,idxM1) = K(idxM1,idxM1) + K11; 42 | % For recursive dynamics 43 | this.body1.wext_i = this.body1.wext_i + f1; 44 | this.body1.Kmdiag = this.body1.Kmdiag + K11; 45 | end 46 | if ~isempty(this.body2) 47 | idxM2 = this.body2.idxM; 48 | f2 = f_(7:12); 49 | K22 = K_(7:12,7:12); 50 | f(idxM2) = f(idxM2) + f2; 51 | K(idxM2,idxM2) = K(idxM2,idxM2) + K22; 52 | % For recursive dynamics 53 | this.body2.wext_i = this.body2.wext_i + f2; 54 | this.body2.Kmdiag = this.body2.Kmdiag + K22; 55 | end 56 | if ~isempty(this.body1) && ~isempty(this.body2) 57 | K12 = K_(1:6,7:12); 58 | K21 = K_(7:12,1:6); 59 | K(idxM1,idxM2) = K(idxM1,idxM2) + K12; 60 | K(idxM2,idxM1) = K(idxM2,idxM1) + K21; 61 | end 62 | end 63 | 64 | %% 65 | function V = computeEnergy_(this,V) 66 | E1 = eye(4); 67 | E2 = eye(4); 68 | x1 = this.x_1; 69 | x2 = this.x_2; 70 | if ~isempty(this.body1) 71 | E1 = this.body1.E_wi; 72 | end 73 | if ~isempty(this.body2) 74 | E2 = this.body2.E_wi; 75 | end 76 | x1_w = E1(1:3,:)*[x1;1]; 77 | x2_w = E2(1:3,:)*[x2;1]; 78 | f_w = x2_w - x1_w; 79 | V = V + 0.5*this.stiffness*(f_w'*f_w); 80 | end 81 | 82 | %% 83 | function y = computeStiffnessProd_(this,x,y) 84 | [~,K] = this.computeFK(); 85 | if ~isempty(this.body1) 86 | idxM1 = this.body1.idxM; 87 | y(idxM1) = y(idxM1) + K(1:6,1:6)*x(idxM1); 88 | end 89 | if ~isempty(this.body2) 90 | idxM2 = this.body2.idxM; 91 | y(idxM2) = y(idxM2) + K(7:12,7:12)*x(idxM2); 92 | end 93 | if ~isempty(this.body1) && ~isempty(this.body2) 94 | y(idxM1) = y(idxM1) + K(1:6,7:12)*x(idxM2); 95 | y(idxM2) = y(idxM2) + K(7:12,1:6)*x(idxM1); 96 | end 97 | end 98 | 99 | %% 100 | function draw_(this) 101 | E1 = eye(4); 102 | E2 = eye(4); 103 | x1 = this.x_1; 104 | x2 = this.x_2; 105 | if ~isempty(this.body1) 106 | E1 = this.body1.E_wi; 107 | end 108 | if ~isempty(this.body2) 109 | E2 = this.body2.E_wi; 110 | end 111 | x1_w = E1(1:3,:)*[x1;1]; 112 | x2_w = E2(1:3,:)*[x2;1]; 113 | xs = [x1_w x2_w]; 114 | plot3(xs(1,:),xs(2,:),xs(3,:),'r-'); 115 | end 116 | end 117 | 118 | %% 119 | methods (Access = private) 120 | %% 121 | function [f,K] = computeFK(this) 122 | E1 = eye(4); 123 | E2 = eye(4); 124 | x1 = this.x_1; 125 | x2 = this.x_2; 126 | if ~isempty(this.body1) 127 | E1 = this.body1.E_wi; 128 | end 129 | if ~isempty(this.body2) 130 | E2 = this.body2.E_wi; 131 | end 132 | R1 = E1(1:3,1:3); 133 | R2 = E2(1:3,1:3); 134 | p1 = E1(1:3,4); 135 | p2 = E2(1:3,4); 136 | G1 = se3.Gamma(x1); 137 | G2 = se3.Gamma(x2); 138 | x1_w = E1(1:3,:)*[x1;1]; 139 | x2_w = E2(1:3,:)*[x2;1]; 140 | f_w = this.stiffness*(x2_w - x1_w); 141 | w_1 = G1'*R1'*f_w; 142 | w_2 = -G2'*R2'*f_w; 143 | f = [w_1;w_2]; 144 | % The Hessian matrix is not symmetric! (see checkDerivatives.m) 145 | dw = zeros(12); 146 | x1b = se3.brac(x1); 147 | x2b = se3.brac(x2); 148 | I = eye(3); 149 | R2R1 = R2'*R1; 150 | R1R2 = R2R1'; 151 | dw( 4: 6, 1: 3) = se3.brac(R1'*(p1 - x2_w)); 152 | dw( 1: 3, 1: 3) = x1b*dw( 4: 6, 1: 3); 153 | dw(10:12, 1: 3) = R2R1*x1b; 154 | dw( 7: 9, 1: 3) = x2b*dw(10:12, 1: 3); 155 | dw( 4: 6, 4: 6) = I; 156 | dw( 1: 3, 4: 6) = x1b; 157 | dw(10:12, 4: 6) = -R2R1; 158 | dw( 7: 9, 4: 6) = x2b*dw(10:12, 4: 6); 159 | dw( 4: 6, 7: 9) = R1R2*x2b; 160 | dw( 1: 3, 7: 9) = x1b*dw( 4: 6, 7: 9); 161 | dw(10:12, 7: 9) = se3.brac(R2'*(p2 - x1_w)); 162 | dw( 7: 9, 7: 9) = x2b*dw(10:12, 7: 9); 163 | dw( 4: 6,10:12) = -R1R2; 164 | dw( 1: 3,10:12) = x1b*dw( 4: 6,10:12); 165 | dw(10:12,10:12) = I; 166 | dw( 7: 9,10:12) = x2b; 167 | K = -0.5*this.stiffness*(dw + dw'); % symmetrize 168 | end 169 | end 170 | end 171 | 172 | -------------------------------------------------------------------------------- /matlab-diff/+redmax/JointUniversal.m: -------------------------------------------------------------------------------- 1 | classdef JointUniversal < redmax.Joint 2 | %JointUniversal A universal joint allowing rotation about X and Y 3 | 4 | %% 5 | properties 6 | end 7 | 8 | %% 9 | methods 10 | %% 11 | function this = JointUniversal(parent,body) 12 | this = this@redmax.Joint(parent,body,2); 13 | end 14 | end 15 | 16 | %% 17 | methods (Access = protected) 18 | %% 19 | function update_(this,deriv) 20 | q = this.q; 21 | qdot = this.qdot; 22 | 23 | [R,dRdq,Rdot,dRdotdq,S,dSdq,Sdot,dSdotdq] = redmax.JointUniversal.XY(q,qdot); 24 | 25 | this.Q(1:3,1:3) = R; 26 | this.A = se3.Ad(this.Q); 27 | 28 | this.Adot(1:3,1:3) = Rdot; 29 | this.Adot(4:6,4:6) = Rdot; 30 | this.S(1:3,1:2) = S; 31 | this.Sdot(1:3,1:2) = Sdot; 32 | 33 | if deriv 34 | for i = 1 : 2 35 | this.dAdq(1:3,1:3,i) = dRdq(:,:,i); 36 | this.dAdq(4:6,4:6,i) = dRdq(:,:,i); 37 | this.dAdotdq(1:3,1:3,i) = dRdotdq(:,:,i); 38 | this.dAdotdq(4:6,4:6,i) = dRdotdq(:,:,i); 39 | this.dSdq(1:3,1:2,i) = dSdq(:,:,i); 40 | this.dSdotdq(1:3,1:2,i) = dSdotdq(:,:,i); 41 | end 42 | end 43 | end 44 | 45 | %% 46 | function draw_(this) 47 | s = 2*min(this.body.getAxisSize()); 48 | p = this.E_wj(1:3,4); 49 | x = this.E_wj(1:3,1); 50 | y = this.E_wj(1:3,2); 51 | px0 = p - s*x; 52 | px1 = p + s*x; 53 | py0 = p - s*y; 54 | py1 = p + s*y; 55 | ps = [px0 px1]; 56 | plot3(ps(1,:),ps(2,:),ps(3,:),'r','LineWidth',2); 57 | ps = [py0 py1]; 58 | plot3(ps(1,:),ps(2,:),ps(3,:),'g','LineWidth',2); 59 | end 60 | end 61 | 62 | methods (Static) 63 | %% 64 | function codegen() 65 | clear; 66 | q = sym('q',[2,1],'real'); 67 | qdot = sym('qdot',[2,1],'real'); 68 | s = sin(q); 69 | c = cos(q); 70 | X1 = [1 0 0; 0 c(1) -s(1); 0 s(1) c(1)]; 71 | Y2 = [c(2) 0 s(2); 0 1 0; -s(2) 0 c(2)]; 72 | R = X1*Y2; 73 | 74 | % dRdq 75 | dRdq = 0*sym('dRdq',[3,3,2],'real'); 76 | for i = 1 : 2 77 | dRdq(:,:,i) = diff(R,q(i)); 78 | end 79 | % Rdot 80 | Rdot = 0*sym('Rdot',[3,3],'real'); 81 | for i = 1 : 2 82 | Rdot = Rdot + dRdq(:,:,i)*qdot(i); 83 | end 84 | Rdot = simplify(Rdot); 85 | % dRdotdq 86 | dRdotdq = 0*sym('dRdotdq',[3,3,2],'real'); 87 | for i = 1 : 2 88 | dRdotdq(:,:,i) = diff(Rdot,q(i)); 89 | end 90 | % S 91 | S = 0*sym('S',[3,2],'real'); 92 | for i = 1 : 2 93 | tmp = simplify(R'*diff(R,q(i))); 94 | S(1:3,i) = [tmp(3,2); tmp(1,3); tmp(2,1)]; 95 | end 96 | % dSdq 97 | dSdq = 0*sym('dSdq',[3,2,2],'real'); 98 | for i = 1 : 2 99 | dSdq(:,:,i) = diff(S,q(i)); 100 | end 101 | % Sdot 102 | Sdot = 0*sym('Sdot',[3,2],'real'); 103 | for i = 1 : 2 104 | Sdot = Sdot + dSdq(:,:,i)*qdot(i); 105 | end 106 | % dSdotdq 107 | dSdotdq = 0*sym('dSdotdq',[3,2,2],'real'); 108 | for i = 1 : 2 109 | dSdotdq(:,:,i) = diff(Sdot,q(i)); 110 | end 111 | % Output 112 | matlabFunction(R,dRdq,Rdot,dRdotdq,... 113 | S,dSdq,Sdot,dSdotdq,... 114 | 'File','XY.m','Optimize',true,'Vars',{q,qdot},... 115 | 'Outputs',{'R','dRdq','Rdot','dRdotdq',... 116 | 'S','dSdq','Sdot','dSdotdq'}); 117 | end 118 | end 119 | 120 | methods (Static, Access = private) 121 | %% 122 | function [R,dRdq,Rdot,dRdotdq,S,dSdq,Sdot,dSdotdq] = XY(in1,in2) 123 | q1 = in1(1,:); 124 | q2 = in1(2,:); 125 | qdot1 = in2(1,:); 126 | qdot2 = in2(2,:); 127 | t2 = cos(q1); 128 | t3 = cos(q2); 129 | t4 = sin(q1); 130 | t5 = sin(q2); 131 | t6 = qdot1.*t2; 132 | t7 = qdot2.*t3; 133 | t8 = qdot1.*t4; 134 | t9 = qdot2.*t5; 135 | t10 = t2.*t3; 136 | t11 = t2.*t5; 137 | t12 = t3.*t4; 138 | t13 = t4.*t5; 139 | t14 = -t5; 140 | t15 = t3.*t6; 141 | t16 = t2.*t7; 142 | t17 = t5.*t6; 143 | t18 = t3.*t8; 144 | t19 = t2.*t9; 145 | t20 = t4.*t7; 146 | t21 = t5.*t8; 147 | t22 = t4.*t9; 148 | t23 = -t7; 149 | t24 = -t8; 150 | t25 = -t9; 151 | t26 = -t10; 152 | t27 = -t11; 153 | t28 = -t12; 154 | R = reshape([t3,t13,t27,0.0,t2,t4,t5,t28,t10],[3,3]); 155 | if nargout > 1 156 | dRdq = reshape([0.0,t11,t13,0.0,-t4,t2,0.0,t26,t28,t14,t12,t26,0.0,0.0,0.0,t3,t13,t27],[3,3,2]); 157 | end 158 | if nargout > 2 159 | t29 = -t15; 160 | t30 = -t16; 161 | t31 = t17+t20; 162 | t32 = t18+t19; 163 | t33 = t22+t29; 164 | t34 = t21+t30; 165 | Rdot = reshape([t25,t31,t34,0.0,t24,t6,t7,t33,-t32],[3,3]); 166 | end 167 | if nargout > 3 168 | dRdotdq = reshape([0.0,t16+t8.*t14,t31,0.0,-t6,t24,0.0,t32,t33,t23,t15-t22,t32,0.0,0.0,0.0,t25,t31,t34],[3,3,2]); 169 | end 170 | if nargout > 4 171 | S = reshape([t3,0.0,t5,0.0,1.0,0.0],[3,2]); 172 | end 173 | if nargout > 5 174 | dSdq = reshape([0.0,0.0,0.0,0.0,0.0,0.0,t14,0.0,t3,0.0,0.0,0.0],[3,2,2]); 175 | end 176 | if nargout > 6 177 | Sdot = reshape([t25,0.0,t7,0.0,0.0,0.0],[3,2]); 178 | end 179 | if nargout > 7 180 | dSdotdq = reshape([0.0,0.0,0.0,0.0,0.0,0.0,t23,0.0,t25,0.0,0.0,0.0],[3,2,2]); 181 | end 182 | end 183 | end 184 | end 185 | -------------------------------------------------------------------------------- /matlab/+redmax/JointRevolute.m: -------------------------------------------------------------------------------- 1 | classdef JointRevolute < redmax.Joint 2 | %% 3 | properties 4 | axis % Axis of rotation 5 | radius % Radius for contact 6 | height % Height for contact 7 | end 8 | 9 | %% 10 | methods 11 | %% 12 | function this = JointRevolute(parent,body,axis) 13 | this = this@redmax.Joint(parent,body,1); 14 | this.axis = axis; 15 | this.radius = 1.0; 16 | this.height = 1.0; 17 | end 18 | 19 | %% 20 | function setGeometry(this,radius,height) 21 | this.radius = radius; 22 | this.height = height; 23 | end 24 | end 25 | 26 | %% 27 | methods (Access = protected) 28 | %% 29 | function update_(this) 30 | this.Q = [se3.aaToMat(this.axis,this.q),[0 0 0]'; 0 0 0 1]; 31 | this.S(1:3) = this.axis; 32 | end 33 | 34 | %% 35 | function generateContacts_(this) 36 | % The contact points will be generated in the 'A' frame, which 37 | % is a frame defined with its Z-axis as the axis of rotation of 38 | % this joint. 39 | E_ja = eye(4); 40 | z = [0 0 1]'; 41 | angle = acos(max(-1.0, min(this.axis'*z, 1.0))); 42 | E_ja(1:3,1:3) = se3.aaToMat(cross(this.axis,z),angle); 43 | E_ia = this.body.E_ij * E_ja; 44 | E_ac = eye(4); % where is the contact frame wrt axis frame 45 | ntheta = 4; 46 | for k = 1 : ntheta 47 | theta = 2*pi*(k-1)/ntheta; 48 | s = sin(theta); 49 | c = cos(theta); 50 | nor = [c s 0]'; 51 | pos = this.radius*nor; 52 | tan = [-s c 0]'; 53 | for i = 1 : 2 54 | z = this.height*(i-1.5); % two ends of the cylinder 55 | pos(3) = z; 56 | bin = cross(tan,nor); 57 | E_ac(1:3,:) = [tan nor bin pos]; 58 | E_ic = E_ia*E_ac; 59 | t = E_ic(1:3,1); 60 | n = E_ic(1:3,2); 61 | b = E_ic(1:3,3); 62 | x = E_ic(1:3,4); 63 | this.contacts{end+1}.pos_i = x; 64 | this.contacts{end}.nor_i = -n; 65 | this.contacts{end}.tan_i = t; 66 | if i == 1 67 | b = -b; 68 | end 69 | this.contacts{end+1}.pos_i = x; 70 | this.contacts{end}.nor_i = b; 71 | this.contacts{end}.tan_i = t; 72 | end 73 | end 74 | % Index for tangent matrix 75 | nt = redmax.Scene.countT(); 76 | t = length(this.contacts); 77 | this.idxT = nt + (1:t); 78 | nt = nt + t; 79 | redmax.Scene.countT(nt); 80 | end 81 | 82 | %% 83 | function T = computeTangentMatrix_(this,T) 84 | for i = 1 : length(this.contacts) 85 | contact = this.contacts{i}; 86 | tan_i = contact.tan_i; 87 | pos_i = contact.pos_i; 88 | T(this.idxT(i),this.body.idxM) = tan_i'*se3.Gamma(pos_i); 89 | if ~isempty(this.parent) 90 | E_pi = this.parent.body.E_iw*this.body.E_wi; 91 | tan_p = E_pi(1:3,:)*[tan_i;0]; 92 | pos_p = E_pi(1:3,:)*[pos_i;1]; 93 | T(this.idxT(i),this.parent.body.idxM) = -tan_p'*se3.Gamma(pos_p); 94 | end 95 | end 96 | end 97 | 98 | %% 99 | function [bl,bu,idx] = computeFrictionLimits_(this,mu,SPathresh,bl,bu,idx) 100 | % Combine normal and binormal, since they share the same 101 | % tangent. 102 | for i0 = 1 : 2 : length(this.contacts) 103 | i1 = i0 + 1; 104 | a0 = abs(this.contacts{i0}.a); 105 | a1 = abs(this.contacts{i1}.a); 106 | a = a0 + a1; 107 | bl(this.idxT(i0)) = -mu*a; 108 | bu(this.idxT(i0)) = mu*a; 109 | if a > SPathresh 110 | idx(end+1) = this.idxT(i0); %#ok 111 | end 112 | end 113 | end 114 | 115 | %% 116 | function draw_(this) 117 | n = 8; 118 | E_ja = eye(4); 119 | z = [0 0 1]'; 120 | angle = acos(max(-1.0, min(this.axis'*z, 1.0))); 121 | E_ja(1:3,1:3) = se3.aaToMat(cross(this.axis,z),angle); 122 | E = this.E_wj*E_ja; 123 | % Cylinder 124 | r = this.radius; 125 | h = this.height; 126 | S = diag([1,1,h,1]); 127 | T = [eye(3),[0 0 -h/2]'; 0 0 0 1]; 128 | [X,Y,Z] = cylinder(r,n); 129 | X = reshape(X,1,2*(n+1)); 130 | Y = reshape(Y,1,2*(n+1)); 131 | Z = reshape(Z,1,2*(n+1)); 132 | XYZ = E*T*S*[X;Y;Z;ones(1,2*(n+1))]; 133 | X0 = reshape(XYZ(1,:),2,n+1); 134 | Y0 = reshape(XYZ(2,:),2,n+1); 135 | Z0 = reshape(XYZ(3,:),2,n+1); 136 | %surf(X,Y,Z,'FaceColor',this.body.color); 137 | % Ends 138 | % https://www.mathworks.com/matlabcentral/answers/320174-plot-a-surface-on-a-circle-and-annulus 139 | [T,R] = meshgrid(linspace(0,2*pi,n+1),linspace(0,r,2)); 140 | X = R.*cos(T); 141 | Y = R.*sin(T); 142 | Z = -h/2*ones(size(R)); 143 | X = reshape(X,1,2*(n+1)); 144 | Y = reshape(Y,1,2*(n+1)); 145 | Z = reshape(Z,1,2*(n+1)); 146 | XYZ = E*[X;Y;Z;ones(1,2*(n+1))]; 147 | X1 = reshape(XYZ(1,:),2,n+1); 148 | Y1 = reshape(XYZ(2,:),2,n+1); 149 | Z1 = reshape(XYZ(3,:),2,n+1); 150 | %surf(X,Y,Z,'FaceColor',this.body.color); 151 | X = R.*cos(T); 152 | Y = R.*sin(T); 153 | Z = h/2*ones(size(R)); 154 | X = reshape(X,1,2*(n+1)); 155 | Y = reshape(Y,1,2*(n+1)); 156 | Z = reshape(Z,1,2*(n+1)); 157 | XYZ = E*[X;Y;Z;ones(1,2*(n+1))]; 158 | X2 = reshape(XYZ(1,:),2,n+1); 159 | Y2 = reshape(XYZ(2,:),2,n+1); 160 | Z2 = reshape(XYZ(3,:),2,n+1); 161 | surf([X0 X1 X2],[Y0 Y1 Y2],[Z0 Z1 Z2],'FaceColor',this.body.color); 162 | end 163 | end 164 | end 165 | --------------------------------------------------------------------------------