Updated the Simulink implementation to work with the current code

.gitignore

@@ -183,3 +183,39 @@ cmake/sdu_controllers-config.cmake
 .vscode
 .vscode/settings.json
 compile_commands.json
+
+
+## Matlab / Simulink files
+# Autosave files
+*.asv
+*.m~
+*.autosave
+*.slx.r*
+*.mdl.r*
+
+# Derived content-obscured files
+*.p
+
+# Compiled MEX files
+*.mex*
+
+# Packaged app and toolbox files
+*.mlappinstall
+*.mltbx
+
+# Deployable archives
+*.ctf
+
+# Generated helpsearch folders
+helpsearch*/
+
+# Code generation folders
+slprj/
+sccprj/
+codegen/
+
+# Cache files
+*.slxc
+
+# Cloud based storage dotfile
+.MATLABDriveTag

config/models/breeding_blanket_handling_robot.yaml

@@ -19,10 +19,10 @@ kinematics:
 
   dh_params:
     type: "classic" # Describes DH convention is used can be either 'classic' (DH) or 'modified' (MDH)
-    a: [0.0, 0.0, 0.468, -0.468, 0.438, 0.0, 0.0]
-    alpha: [0.0, -1.5707963267948966, 0.0, -1.5707963267948966, 0.0, -1.5707963267948966, 0.0]
-    theta: [-1.5707963267948966, 0.0, 0.0, 0.0, -1.5707963267948966, 0.0, 0.0]
-    d: [-0.0, 0.0, 0.93, 0.0, 3.0323, 0.0, 0.0]
+    a: [0.0, 0.0, 0.468, -0.468, 0.438, 0.0, 0.0, 1.845]
+    alpha: [0.0, -1.5707963267948966, 0.0, -1.5707963267948966, 0.0, -1.5707963267948966, 0.0, 0.0]
+    theta: [-1.5707963267948966, 0.0, 0.0, 0.0, -1.5707963267948966, 0.0, 0.0, -1.5707963267948966]
+    d: [-0.0, 0.0, 0.93, 0.0, 3.0323, 0.0, 0.0, 0.0]
 
 inertial:
   joint_1:
@@ -109,6 +109,18 @@ inertial:
       ixz: 0.0000000000000000
       iyz: 0.0000000000000000
 
+  joint_8:
+    type: "fixed"
+    mass: 0 # kg
+    com: [0.0, 0.0, 0.0] # meters [m]
+    inertia:
+      ixx: 0.0
+      iyy: 0.0
+      izz: 0.0
+      ixy: 0.0
+      ixz: 0.0
+      iyz: 0.0
+
 joint_limits:
   joint_1:
     max_torque: 1000.0 # N

config/models/breeding_blanket_handling_robot_fixed.yaml

@@ -0,0 +1,172 @@
+dof: 6 # Number of degrees of freedom
+robot_name: breeding_blanket_handling_robot
+
+# Model specific notes:
+# -
+
+# The kinematics can be described as DH parameters and or as 6D frames.
+# If both are available set the preferred 'type' to either 'DH' or 'Frames'.
+kinematics:
+  type: "DH" # selects the kinematic format used
+
+  # The inertial reference frame describes where the reference frame of the inertial parameters are
+  # placed. If you use classic DH parameters, this frame will be placed at the end of the link. If
+  # you have described the robot with Modified DH parameters the frame will be at the start of the link.
+  # Finally if you use 6D frames to describe the robot kinematics, the inertial reference frame is then
+  # given by the placement of the frames which is also typically at the end or start of the link.
+  inertial_ref_frame: "end_of_link_like_DH" # can either be "end_of_link_like_DH" or "start_of_link_like_MDH"
+
+
+  dh_params:
+    type: "classic" # Describes DH convention is used can be either 'classic' (DH) or 'modified' (MDH)
+    a: [0.0, 0.0, 0.468, -0.468, 0.438, 0.0, 0.0, 1.845]
+    alpha: [0.0, -1.5707963267948966, 0.0, -1.5707963267948966, 0.0, -1.5707963267948966, 0.0, 0.0]
+    theta: [-1.5707963267948966, 0.0, 0.0, 0.0, -1.5707963267948966, 0.0, 0.0, -1.5707963267948966]
+    d: [-0.0, 0.0, 0.93, 0.0, 3.0323, 0.0, 0.0, 0.0]
+
+inertial:
+  joint_1:
+    type: "prismatic"
+    mass: 3523.6000 # kg
+    com: [0.0462320000000003, 4.6455000000000002, 0.7246200000000000] # meters [m]
+    inertia:
+      ixx: 15940.7319999999999709
+      iyy: 4172.8239999999996144
+      izz: 18582.5329999999994470
+      ixy: 108.1039999999992745
+      ixz: -141.9999999999999716
+      iyz: 233.7580000000000098
+
+  joint_2:
+    type: "revolute"
+    mass: 10415.0000  # kg
+    com: [-0.0000267980000000, -0.9320799999999998, 3.2787999999999999] # meters [m]
+    inertia:
+      ixx: 40567.7660000000032596
+      iyy: 39813.9970000000030268
+      izz: 3000.9760000000001128
+      ixy: 0.2490000000000611
+      ixz: 998.5159999999999627
+      iyz: -0.6270000000022542
+
+  joint_3:
+    type: "prismatic"
+    mass: 5509.2000 # kg
+    com: [-0.4842530000000000, -0.9025200000000000, -0.0028617000000000] # meters [m]
+    inertia:
+      ixx: 2908.0639999999998508
+      iyy: 2113.3659999999999854
+      izz: 2919.8499999999999091
+      ixy: 6.8049999999999997
+      ixz: 0.2550000000000000
+      iyz: -109.9599999999999937
+
+  joint_4:
+    type: "fixed"
+    mass: 2987.8000 # kg
+    com: [0.0259800000000000, 0.0000000004822200, 0.0515730000000000] # meters [m]
+    inertia:
+      ixx: 705.0610000000000355
+      iyy: 522.9439999999999600
+      izz: 655.8440000000000509
+      ixy: 0.0000000000000000
+      ixz: 0.0000000000000000
+      iyz: -12.2159999999999904
+
+  joint_5:
+    type: "revolute"
+    mass: 4453.5000 # kg
+    com: [-0.2682599999999999, -0.0000000687380000, -2.0652500000000003] # meters [m]
+    inertia:
+      ixx: 4341.2989999999999782
+      iyy: 4547.9350000000004002
+      izz: 423.3799999999999955
+      ixy: 0.0000000000000000
+      ixz: -728.8680000000000518
+      iyz: 0.0000000000000000
+
+  joint_6:
+    type: "revolute"
+    mass: 2501.6000 # kg
+    com: [-0.0000000000007461, 0.6950800000000000, -0.0000000000000111] # meters [m]
+    inertia:
+      ixx: 938.3519999999999754
+      iyy: 134.5780000000000030
+      izz: 1006.2380000000000564
+      ixy: 0.0000000000000000
+      ixz: 0.0000000000000000
+      iyz: 0.0000000000000534
+
+  joint_7:
+    type: "revolute"
+    mass: 2359.6000 # kg
+    com: [0.0000002655700000, -0.8878200000000001, -0.0000056360000000] # meters [m]
+    inertia:
+      ixx: 572.3419999999999845
+      iyy: 184.7949999999999875
+      izz: 630.2889999999999873
+      ixy: -0.0120000000000000
+      ixz: 0.0000000000000000
+      iyz: 0.0000000000000000
+
+  joint_8:
+    type: "fixed"
+    mass: 0 # kg
+    com: [0.0, 0.0, 0.0] # meters [m]
+    inertia:
+      ixx: 0.0
+      iyy: 0.0
+      izz: 0.0
+      ixy: 0.0
+      ixz: 0.0
+      iyz: 0.0
+
+joint_limits:
+  joint_1:
+    max_torque: 1000.0 # N
+    max_velocity: 0.01 # m/s
+    max_acceleration: 40.0 # m/s²
+    max_position: 8.0 # m
+    min_position: -25.0 # m
+
+  joint_2:
+    max_torque: 1000.0 # Nm
+    max_velocity: 0.0070 # rad/s
+    max_acceleration: 40.0 # rad/s²
+    max_position: 0.1396 # rad
+    min_position: -0.1396 # rad
+
+  joint_3:
+    max_torque: 1000.0 # N
+    max_velocity: 0.005  # m/s
+    max_acceleration: 40.0 # m/s²
+    max_position: 3.7 # m
+    min_position: 0 # m
+
+  joint_4:
+    max_torque: 1000.0 # Nm
+    max_velocity: 0.002 # rad/s
+    max_acceleration: 40.0 # rad/s²
+    max_position: 0.0349 # rad
+    min_position: -0.0349 # rad
+
+  joint_5:
+    max_torque: 1000.0 # Nm
+    max_velocity: 0.88 # rad/s
+    max_acceleration: 40.0 # rad/s²
+    max_position: 6.28318530718 # rad
+    min_position: -6.28318530718  # rad
+
+  joint_6:
+    max_torque: 1000.0 # Nm
+    max_velocity: 0.88 # rad/s
+    max_acceleration: 40.0 # rad/s²
+    max_position: 6.28318530718 # rad
+    min_position: -6.28318530718 # rad
+
+  joint_7:
+    max_torque: 1000.0 # Nm
+    max_velocity: 0.002 # rad/s
+    max_acceleration: 40.0 # rad/s²
+    max_position: 0.1396 # rad
+    min_position: -0.1396 # rad

examples/simulation/CMakeLists.txt

@@ -8,3 +8,4 @@ add_example(impedance_controller ur/cpp)
 add_example(eurofusion_joint_motion_controller eurofusion/cpp)
 add_example(eurofusion_robot_rnea eurofusion/cpp)
 add_example(eurofusion_force_control_velocity eurofusion/cpp)
+add_example(eurofusion_cartesian_motion_controller eurofusion/cpp)

examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp

@@ -0,0 +1,137 @@
+#include <Eigen/Dense>
+#include <fstream>
+#include <iostream>
+#include <sdu_controllers/controllers/operational_space_controller.hpp>
+#include <sdu_controllers/math/forward_dynamics.hpp>
+#include <sdu_controllers/math/inverse_dynamics_joint_space.hpp>
+#include <sdu_controllers/kinematics/forward_kinematics.hpp>
+#include <sdu_controllers/models/parameter_robot_model.hpp>
+#include <sdu_controllers/safety/safety_verifier.hpp>
+#include <sdu_controllers/utils/utility.hpp>
+
+using namespace csv;
+using namespace Eigen;
+using namespace sdu_controllers;
+using namespace sdu_controllers::utils;
+
+int main()
+{
+  // Setup writing of output trajectory to csv.
+  std::ofstream output_filestream;
+  output_filestream.open("output_cartesian.csv");
+  auto csv_writer = make_csv_writer(output_filestream);
+
+  // Initialize robot model and parameters
+  auto robot_model = std::make_shared<models::ParameterRobotModel>(utils::ConfigFolder::find_config_file("breeding_blanket_handling_robot_fixed.yaml"));
+
+  double freq = 1000.0;
+  double dt = 1.0 / freq;
+
+  double Kp_pos_value = 10.0;
+  double Kp_orient_value = 1;
+  double Kd_pos_value = 20.0;
+  double Kd_orient_value = 2;
+  double N_value = 0;
+  uint16_t ROBOT_DOF = robot_model->get_dof();
+
+  VectorXd Kp_pos_vec = VectorXd::Ones(3) * Kp_pos_value;
+  VectorXd Kp_orient_vec = VectorXd::Ones(3) * Kp_orient_value;
+  VectorXd Kd_pos_vec = VectorXd::Ones(3) * Kd_pos_value;
+  VectorXd Kd_orient_vec = VectorXd::Ones(3) * Kd_orient_value;
+  VectorXd N_vec = VectorXd::Ones(6) * N_value;
+
+  MatrixXd Kp = MatrixXd::Zero(6, 6);
+  Kp.setIdentity();
+  Kp.block<3, 3>(0,0) = Kp_pos_vec.asDiagonal();
+  Kp.block<3, 3>(3,3) = Kp_orient_vec.asDiagonal();
+  MatrixXd Kd = MatrixXd::Zero(6, 6);
+  Kd.setIdentity();
+  Kd.block<3, 3>(0,0) = Kd_pos_vec.asDiagonal();
+  Kd.block<3, 3>(3,3) = Kd_orient_vec.asDiagonal();
+
+  controllers::OperationalSpaceController osc_controller(Kp, Kd, robot_model);
+  osc_controller.set_kappa(1e-2);
+  //math::InverseDynamicsJointSpace inv_dyn_jnt_space(robot_model);
+  //math::ForwardDynamics fwd_dyn(robot_model);
+
+  VectorXd x_d(6);
+  VectorXd dx_d(6);
+  VectorXd ddx_d(6);
+
+  VectorXd q(ROBOT_DOF), q0(ROBOT_DOF);
+  VectorXd dq(ROBOT_DOF);
+  Vector<double, 6> he = VectorXd::Zero(6);
+  q << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+  q0 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+  dq << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+  // output to csv
+  output_filestream << "q0,q1,q2,q3,q4,q5,x,y,z,rx,ry,rz" << std::endl;;
+
+  // Stay put at the initial position
+  // Control loop for 10s
+  for (size_t j=0; j < freq * 20.; j++)  // (const std::vector<double>& trajectory_point : input_trajectory)
+  {
+    if (j % int(freq) == 0)
+      std::cout << "Time: " << j * dt << std::endl;
+
+    // Desired
+    Eigen::Matrix4d T0 = robot_model->get_fk_solver().forward_kinematics(q0);
+    VectorXd pos0 = T0.block<3, 1>(0, 3);
+    Matrix3d rot_mat0 = T0.block<3,3>(0, 0);
+    Vector3d rpy_zyz0 = rot_mat0.eulerAngles(2, 1, 2); // ZYZ representation
+
+    Vector3d pos_desired = pos0;
+    pos_desired[0] += 0.1;
+    pos_desired[1] += -0.1;
+    pos_desired[2] += 2;
+
+    Vector3d rpy_zyz_desired = rpy_zyz0;
+    rpy_zyz_desired[0] += M_PI / 8;
+    rpy_zyz_desired[1] += 0;
+    rpy_zyz_desired[2] += M_PI / 2;
+
+    x_d << pos_desired, rpy_zyz_desired;
+    dx_d.setZero();
+    ddx_d.setZero();
+
+    // std::cout << "x_d: " << x_d << std::endl;
+
+    // Add noise to q and dq
+    VectorXd q_meas = q;
+    VectorXd dq_meas = dq;
+    //add_noise_to_vector(q_meas, 0.0, 0.001);
+    //add_noise_to_vector(dq_meas, 0.0, 0.001);
+
+    // std::cout << "before all fk" << std::endl;
+    // std::vector<Eigen::Matrix4d> TTT = robot_model->get_fk_solver().forward_kinematics_all(q0);
+    // std::cout << "after all fk" << std::endl;
+    // std::cout << TTT.at(4) << std::endl;
+
+    // Controller
+    osc_controller.step(x_d, dx_d, ddx_d, q_meas, dq_meas);
+    VectorXd y = osc_controller.get_output();
+    // std::cout << "y: " << y << std::endl;
+    VectorXd tau = robot_model->inverse_dynamics(q_meas, dq_meas, y, he);
+    // std::cout << "tau: " << tau << std::endl;
+
+    // Simulation
+    VectorXd ddq = robot_model->forward_dynamics(q, dq, tau);
+    // integrate to get velocity
+    dq += ddq * dt;
+    // integrate to get position
+    q += dq * dt;
+
+    // std::cout << "q:" << q << std::endl;
+    MatrixXd T = robot_model->get_fk_solver().forward_kinematics(q);
+    VectorXd pos = T.block<3, 1>(0, 3);
+    // std::cout << "pos:" << pos << std::endl;
+    Matrix3d rot_mat = T.block<3,3>(0, 0);
+    Vector3d rpy_zyz = rot_mat.eulerAngles(2, 1, 2); // ZYZ representation
+    // std::cout << "rpy_zyz:" << rpy_zyz << std::endl;
+    VectorXd temp(q.size()+pos.size()+rpy_zyz.size());
+    temp << q, pos, rpy_zyz;
+    csv_writer << eigen_to_std_vector(temp);
+  }
+  output_filestream.close();
+}