Pineapple

CMakeLists.txt

@@ -165,21 +165,87 @@ file(GLOB SOURCES "${DIR}*.c")
 
 add_library(${PROJECT_NAME} SHARED src/wrapper.c ${SOURCES})
 
-### ----- Generate dynamics for Pineapple 8DoF ----- ###
-project(pineapple_8dof) # Change this for each model
+### ----- Generate dynamics for Pineapple v0 ----- ###
+project(pineapple_v0) # Change this for each model
 
-set(DIR models/pineapple/generated_code/pineapple_8dof/) # Change if not the default
+set(DIR models/pineapple/generated_code/pineapple_v0/) # Change if not the default
 
 include_directories(${DIR})
 
 file(GLOB SOURCES "${DIR}*.c") 
 
 add_library(${PROJECT_NAME} SHARED src/wrapper.c ${SOURCES})
 
-### ----- Generate dynamics for Pineapple 8DoF Quaternion Kinematics ----- ###
-project(pineapple_8dof_quat) # Change this for each model
+### ----- Generate dynamics for Pineapple v0 Quaternion Kinematics ----- ###
+project(pineapple_quat_v0) # Change this for each model
 
-set(DIR models/pineapple/generated_code/pineapple_8dof_quat/) # Change if not the default
+set(DIR models/pineapple/generated_code/pineapple_quat_v0/) # Change if not the default
+
+include_directories(${DIR})
+
+file(GLOB SOURCES "${DIR}*.c") 
+
+add_library(${PROJECT_NAME} SHARED src/wrapper.c ${SOURCES})
+
+### ----- Generate dynamics for Pineapple v1 ----- ###
+project(pineapple_v1) # Change this for each model
+
+set(DIR models/pineapple/generated_code/pineapple_v1/) # Change if not the default
+
+include_directories(${DIR})
+
+file(GLOB SOURCES "${DIR}*.c") 
+
+add_library(${PROJECT_NAME} SHARED src/wrapper.c ${SOURCES})
+
+### ----- Generate dynamics for Pineapple v1 Quaternion Kinematics ----- ###
+project(pineapple_quat_v1) # Change this for each model
+
+set(DIR models/pineapple/generated_code/pineapple_quat_v1/) # Change if not the default
+
+include_directories(${DIR})
+
+file(GLOB SOURCES "${DIR}*.c") 
+
+add_library(${PROJECT_NAME} SHARED src/wrapper.c ${SOURCES})
+
+### ----- Generate dynamics for Pineapple v2 ----- ###
+project(pineapple_v2) # Change this for each model
+
+set(DIR models/pineapple/generated_code/pineapple_v2/) # Change if not the default
+
+include_directories(${DIR})
+
+file(GLOB SOURCES "${DIR}*.c") 
+
+add_library(${PROJECT_NAME} SHARED src/wrapper.c ${SOURCES})
+
+### ----- Generate dynamics for Pineapple v2 Quaternion Kinematics ----- ###
+project(pineapple_quat_v2) # Change this for each model
+
+set(DIR models/pineapple/generated_code/pineapple_quat_v2/) # Change if not the default
+
+include_directories(${DIR})
+
+file(GLOB SOURCES "${DIR}*.c") 
+
+add_library(${PROJECT_NAME} SHARED src/wrapper.c ${SOURCES})
+
+### ----- Generate dynamics for Pineapple v3 Quaternion Kinematics ----- ###
+project(pineapple_quat_v3) # Change this for each model
+
+set(DIR models/pineapple/generated_code/pineapple_quat_v3/) # Change if not the default
+
+include_directories(${DIR})
+
+file(GLOB SOURCES "${DIR}*.c") 
+
+add_library(${PROJECT_NAME} SHARED src/wrapper.c ${SOURCES})
+
+### ----- Generate dynamics for Pineapple v3 with gripper Quaternion Kinematics ----- ###
+project(pineapple_quat_v3_gripper) # Change this for each model
+
+set(DIR models/pineapple/generated_code/pineapple_quat_v3_gripper/) # Change if not the default
 
 include_directories(${DIR})
 

Project.toml

@@ -8,4 +8,13 @@ FiniteDifferences = "26cc04aa-876d-5657-8c51-4c34ba976000"
 ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 Libdl = "8f399da3-3557-5675-b5ff-fb832c97cbdb"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
-Revise = "295af30f-e4ad-537b-8983-00126c2a3abe"
+StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
+
+[weakdeps]
+RigidBodyDynamics = "366cf18f-59d5-4db9-a4de-86a9f6786172"
+
+[extensions]
+RigidBodyDynamicsExt = "RigidBodyDynamics"
+
+[compat]
+julia = "1.9"

ext/RigidBodyDynamicsExt.jl

@@ -0,0 +1,32 @@
+module RigidBodyDynamicsExt
+
+using PinnZoo
+using RigidBodyDynamics
+
+function _add_rigid_body_dynamics_order!(orders, conversions, urdf_path)
+    try
+        floating = length(orders[:nominal].config_names) >= 7 && orders[:nominal].config_names[4] == :q_w # Hacky check for whether this model should be floating base
+        robot = parse_urdf(urdf_path; remove_fixed_tree_joints=false, floating=floating)
+        state = MechanismState(robot)
+        config_names = [Symbol(joints(robot)[id].name) for id in state.q_index_to_joint_id]
+        vel_names = [Symbol(joints(robot)[id].name) for id in state.v_index_to_joint_id]
+        for joint in joints(robot)
+            if typeof(joint.joint_type) <: QuaternionFloating
+                config_names[state.qranges[joint]] = [:q_w, :q_x, :q_y, :q_z, :x, :y, :z]
+                vel_names[state.vranges[joint]] = [:ang_v_x, :ang_v_y, :ang_v_z, :lin_v_x, :lin_v_y, :lin_v_z]
+            end
+        end
+        torque_names = [name for name in vel_names if name in orders[:nominal].torque_names]
+        orders[:rigidBodyDynamics] = StateOrder(config_names, vel_names, torque_names)
+        generate_conversions(orders, conversions)
+    catch err
+        @warn "RigidBodyDynamics state order not added; nominal/Pinocchio conversions are unchanged" urdf_path exception=(err,)
+    end
+    return nothing
+end
+
+function __init__()
+    push!(PinnZoo.GENERATE_ORDER_HOOKS, _add_rigid_body_dynamics_order!)
+end
+
+end

models/cartpole/generated_code/C_func.c

@@ -1,4 +1,4 @@
-/* This file was automatically generated by CasADi 3.6.7.
+/* This file was automatically generated by CasADi 3.7.2.
  *  It consists of: 
  *   1) content generated by CasADi runtime: not copyrighted
  *   2) template code copied from CasADi source: permissively licensed (MIT-0)
@@ -48,8 +48,8 @@ extern "C" {
   #endif
 #endif
 
-static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
-static const casadi_int casadi_s1[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s0[3] = {4, 1, 1};
+static const casadi_int casadi_s1[3] = {2, 1, 1};
 
 /* C_func:(i0[4])->(o0[2]) */
 static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {

models/cartpole/generated_code/C_func.h

@@ -1,4 +1,4 @@
-/* This file was automatically generated by CasADi 3.6.7.
+/* This file was automatically generated by CasADi 3.7.2.
  *  It consists of: 
  *   1) content generated by CasADi runtime: not copyrighted
  *   2) template code copied from CasADi source: permissively licensed (MIT-0)
@@ -37,7 +37,7 @@ int C_func_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw,
 #define C_func_SZ_ARG 1
 #define C_func_SZ_RES 1
 #define C_func_SZ_IW 0
-#define C_func_SZ_W 7
+#define C_func_SZ_W 0
 #ifdef __cplusplus
 } /* extern "C" */
 #endif

models/cartpole/generated_code/M_func.c

@@ -1,4 +1,4 @@
-/* This file was automatically generated by CasADi 3.6.7.
+/* This file was automatically generated by CasADi 3.7.2.
  *  It consists of: 
  *   1) content generated by CasADi runtime: not copyrighted
  *   2) template code copied from CasADi source: permissively licensed (MIT-0)
@@ -48,8 +48,8 @@ extern "C" {
   #endif
 #endif
 
-static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
-static const casadi_int casadi_s1[9] = {2, 2, 0, 2, 4, 0, 1, 0, 1};
+static const casadi_int casadi_s0[3] = {4, 1, 1};
+static const casadi_int casadi_s1[3] = {2, 2, 1};
 
 /* M_func:(i0[4])->(o0[2x2]) */
 static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {

models/cartpole/generated_code/M_func.h

@@ -1,4 +1,4 @@
-/* This file was automatically generated by CasADi 3.6.7.
+/* This file was automatically generated by CasADi 3.7.2.
  *  It consists of: 
  *   1) content generated by CasADi runtime: not copyrighted
  *   2) template code copied from CasADi source: permissively licensed (MIT-0)
@@ -37,7 +37,7 @@ int M_func_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw,
 #define M_func_SZ_ARG 1
 #define M_func_SZ_RES 1
 #define M_func_SZ_IW 0
-#define M_func_SZ_W 1
+#define M_func_SZ_W 0
 #ifdef __cplusplus
 } /* extern "C" */
 #endif

models/cartpole/generated_code/M_jvp.c

@@ -1,4 +1,4 @@
-/* This file was automatically generated by CasADi 3.6.7.
+/* This file was automatically generated by CasADi 3.7.2.
  *  It consists of: 
  *   1) content generated by CasADi runtime: not copyrighted
  *   2) template code copied from CasADi source: permissively licensed (MIT-0)
@@ -49,9 +49,9 @@ extern "C" {
   #endif
 #endif
 
-static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
-static const casadi_int casadi_s1[6] = {2, 1, 0, 2, 0, 1};
-static const casadi_int casadi_s2[15] = {2, 4, 0, 2, 4, 6, 8, 0, 1, 0, 1, 0, 1, 0, 1};
+static const casadi_int casadi_s0[3] = {4, 1, 1};
+static const casadi_int casadi_s1[3] = {2, 1, 1};
+static const casadi_int casadi_s2[3] = {2, 4, 1};
 
 /* M_jvp:(i0[4],i1[2])->(o0[2x4]) */
 static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {

models/cartpole/generated_code/M_jvp.h

@@ -1,4 +1,4 @@
-/* This file was automatically generated by CasADi 3.6.7.
+/* This file was automatically generated by CasADi 3.7.2.
  *  It consists of: 
  *   1) content generated by CasADi runtime: not copyrighted
  *   2) template code copied from CasADi source: permissively licensed (MIT-0)
@@ -37,7 +37,7 @@ int M_jvp_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw,
 #define M_jvp_SZ_ARG 2
 #define M_jvp_SZ_RES 1
 #define M_jvp_SZ_IW 0
-#define M_jvp_SZ_W 3
+#define M_jvp_SZ_W 0
 #ifdef __cplusplus
 } /* extern "C" */
 #endif

models/cartpole/generated_code/dynamics.c

@@ -1,4 +1,4 @@
-/* This file was automatically generated by CasADi 3.6.7.
+/* This file was automatically generated by CasADi 3.7.2.
  *  It consists of: 
  *   1) content generated by CasADi runtime: not copyrighted
  *   2) template code copied from CasADi source: permissively licensed (MIT-0)
@@ -51,54 +51,54 @@ extern "C" {
 
 casadi_real casadi_sq(casadi_real x) { return x*x;}
 
-static const casadi_int casadi_s0[8] = {4, 1, 0, 4, 0, 1, 2, 3};
-static const casadi_int casadi_s1[6] = {2, 1, 0, 2, 0, 1};
+static const casadi_int casadi_s0[3] = {4, 1, 1};
+static const casadi_int casadi_s1[3] = {2, 1, 1};
 
 /* dynamics:(i0[4],i1[2])->(o0[4]) */
 static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) {
-  casadi_real a0, a1, a10, a2, a3, a4, a5, a6, a7, a8, a9;
-  a0=arg[0]? arg[0][2] : 0;
-  if (res[0]!=0) res[0][0]=a0;
-  a1=arg[0]? arg[0][3] : 0;
-  if (res[0]!=0) res[0][1]=a1;
-  a2=arg[1]? arg[1][0] : 0;
-  a3=arg[0]? arg[0][1] : 0;
-  a4=cos(a3);
-  a3=sin(a3);
-  a5=(a3*a0);
-  a6=(a1*a5);
-  a7=arg[1]? arg[1][1] : 0;
-  a0=(a4*a0);
-  a8=(a0-a1);
-  a9=(a5*a8);
-  a10=(a0*a5);
-  a9=(a9-a10);
-  a7=(a7-a9);
-  a6=(a6-a7);
-  a6=(a4*a6);
-  a8=(a1*a8);
-  a0=(a1*a0);
-  a8=(a8-a0);
-  a8=(a3*a8);
-  a6=(a6-a8);
-  a2=(a2-a6);
-  a6=1.;
-  a8=casadi_sq(a3);
-  a6=(a6+a8);
-  a2=(a2/a6);
-  a8=9.8100000000000005e+00;
-  a0=(a4*a3);
-  a0=(a0/a6);
-  a0=(a8*a0);
-  a2=(a2+a0);
-  if (res[0]!=0) res[0][2]=a2;
-  a4=(a4*a2);
-  a8=(a8*a3);
-  a4=(a4+a8);
-  a1=(a1*a5);
-  a4=(a4-a1);
-  a7=(a7+a4);
-  if (res[0]!=0) res[0][3]=a7;
+  casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10;
+  a00=arg[0]? arg[0][2] : 0;
+  if (res[0]!=0) res[0][0]=a00;
+  a01=arg[0]? arg[0][3] : 0;
+  if (res[0]!=0) res[0][1]=a01;
+  a02=arg[1]? arg[1][0] : 0;
+  a03=arg[0]? arg[0][1] : 0;
+  a04=cos(a03);
+  a03=sin(a03);
+  a05=(a03*a00);
+  a06=(a01*a05);
+  a07=arg[1]? arg[1][1] : 0;
+  a00=(a04*a00);
+  a08=(a00-a01);
+  a09=(a05*a08);
+  a10=(a00*a05);
+  a09=(a09-a10);
+  a07=(a07-a09);
+  a06=(a06-a07);
+  a06=(a04*a06);
+  a08=(a01*a08);
+  a00=(a01*a00);
+  a08=(a08-a00);
+  a08=(a03*a08);
+  a06=(a06-a08);
+  a02=(a02-a06);
+  a06=1.;
+  a08=casadi_sq(a03);
+  a06=(a06+a08);
+  a02=(a02/a06);
+  a08=9.8100000000000005e+00;
+  a00=(a04*a03);
+  a00=(a00/a06);
+  a00=(a08*a00);
+  a02=(a02+a00);
+  if (res[0]!=0) res[0][2]=a02;
+  a04=(a04*a02);
+  a08=(a08*a03);
+  a04=(a04+a08);
+  a01=(a01*a05);
+  a04=(a04-a01);
+  a07=(a07+a04);
+  if (res[0]!=0) res[0][3]=a07;
   return 0;
 }
 

models/cartpole/generated_code/dynamics.h

@@ -1,4 +1,4 @@
-/* This file was automatically generated by CasADi 3.6.7.
+/* This file was automatically generated by CasADi 3.7.2.
  *  It consists of: 
  *   1) content generated by CasADi runtime: not copyrighted
  *   2) template code copied from CasADi source: permissively licensed (MIT-0)
@@ -37,7 +37,7 @@ int dynamics_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_i
 #define dynamics_SZ_ARG 2
 #define dynamics_SZ_RES 1
 #define dynamics_SZ_IW 0
-#define dynamics_SZ_W 11
+#define dynamics_SZ_W 0
 #ifdef __cplusplus
 } /* extern "C" */
 #endif