NASA_Challenge_[@xfiderek] Add Trick simulation of canadarm2 (space-r…

…os#42).

Adds Trick simulation of SSRMS, calculating forward dynamics of the arm.
It features friction model taking into account Stribeck effect.
It uses RBDL as a backend library to solve forward dynamics equations.

ros_trick/trick_src/SIM_trick_canadarm/RUN_2DPlanar/input.py

@@ -0,0 +1,31 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+trick.real_time_enable()
+trick.exec_set_software_frame(0.02)
+
+trick.exec_set_enable_freeze(True)
+trick.exec_set_freeze_command(True)
+
+
+trick.var_set_copy_mode(2)
+trick.var_server_set_port(49765)
+trick_message.mtcout.init()
+trick.message_subscribe(trick_message.mtcout)
+trick_message.separate_thread_set_enabled(True)
+simControlPanel = trick.SimControlPanel()
+trick.add_external_application(simControlPanel)
+trick_view = trick.TrickView()
+trick.add_external_application(trick_view)

ros_trick/trick_src/SIM_trick_canadarm/S_define

@@ -0,0 +1,49 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+// 
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// 
+//     https://www.apache.org/licenses/LICENSE-2.0
+// 
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/************************TRICK HEADER*************************
+PURPOSE:
+    ( Simulate dynamics of SSRMS )
+LIBRARY DEPENDENCIES:
+    (
+        (manipulator/manipulator.cc)
+    )
+*************************************************************/
+#include "sim_objects/default_trick_sys.sm"
+
+##include "include/trick/exec_proto.h"
+##include "manipulator/manipulator.hh"
+
+class ManipulatorSimObject : public Trick::SimObject
+{
+
+    public:
+        Manip manip;
+
+        ManipulatorSimObject(): manip()
+        {
+            ("default_data")      manip.defaultData();
+            ("derivative")        manip.stateDeriv();
+            ("integration")       trick_ret = manip.stateInteg();
+        }
+
+};
+
+ManipulatorSimObject CanadarmManip;
+
+IntegLoop armIntegLoop(0.01) CanadarmManip;
+
+void create_connections() {
+    armIntegLoop.getIntegrator(Runge_Kutta_4, 2*CanadarmManip.manip.NDOF);
+}

ros_trick/trick_src/SIM_trick_canadarm/S_overrides.mk

@@ -0,0 +1,18 @@
+# Copyright 2024 Blazej Fiderek (xfiderek)
+# 
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+# 
+#     https://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+TRICK_CFLAGS += -I./models -I${TRICK_HOME} -I/usr/include/eigen3 -g -O0 -Wall -Wextra -Wshadow
+TRICK_CXXFLAGS += -I./models -I${TRICK_HOME} -I/usr/include/eigen3 -g -O0 -Wall -Wextra -Wshadow
+TRICK_USER_LINK_LIBS += -lrbdl -lrbdl_urdfreader
+

ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.cc

@@ -0,0 +1,112 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     https://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "manipulator/manipulator.hh"
+#include "trick/integrator_c_intf.h"
+
+Manip::Manip() : rbdl_model()
+{
+  RigidBodyDynamics::Addons::URDFReadFromFile("./SSRMS_Canadarm2.urdf.xacro", &rbdl_model, false);
+  std::cout << "Loaded URDF model into trick";
+  std::cout << RigidBodyDynamics::Utils::GetModelDOFOverview(rbdl_model);
+
+  rbdl_model.gravity = RigidBodyDynamics::Math::Vector3d::Zero();
+
+  q_vec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size);
+  q_dotvec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size);
+  q_ddotvec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size);
+  torque_vec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size);
+
+  std::cout << "starting Trick Canadarm simulation" << std::endl;
+}
+
+int Manip::defaultData()
+{
+  for (size_t i = 0; i < NDOF; ++i)
+  {
+    q[i] = 0.0;
+    q_dot[i] = 0.0;
+    q_dotdot[i] = 0.0;
+    input_torque[i] = 0.0;
+    applied_torque[i] = 0.0;
+    friction_torque[i] = 0.0;
+    q_vec_[i] = 0.0;
+    q_dotvec_[i] = 0.0;
+    q_ddotvec_[i] = 0.0;
+    torque_vec_[i] = 0.0;
+    breakaway_friction_torque[i] = 2.5;
+    coloumb_friction_torque[i] = 2.0;
+    breakaway_friction_velocity[i] = 0.005;
+    coulomb_velocity_threshold[i] = breakaway_friction_velocity[i] / 10;
+    stribeck_velocity_threshold[i] = M_SQRT2 * breakaway_friction_torque[i];
+    viscous_friction_coefficient[i] = 0.001;
+  }
+
+  return (0);
+}
+
+int Manip::forwardDynamics()
+{
+  // calculate torques and store as RBDL vector as well
+  for (size_t i = 0; i < NDOF; ++i)
+  {
+    // https://www.mathworks.com/help/simscape/ref/rotationalfriction.html
+    double curr_to_stribeck_vel = q_dot[i] / stribeck_velocity_threshold[i];
+    double curr_to_coulomb_vel = q_dot[i] / coulomb_velocity_threshold[i];
+    friction_torque[i] = M_SQRT2 * M_E * (breakaway_friction_torque[i] - coloumb_friction_torque[i]) *
+                         std::exp(-std::pow(curr_to_stribeck_vel, 2)) * curr_to_stribeck_vel;
+    friction_torque[i] += coloumb_friction_torque[i] * std::tanh(curr_to_coulomb_vel);
+    friction_torque[i] += viscous_friction_coefficient[i] * q_dot[i];
+
+    applied_torque[i] = input_torque[i] - friction_torque[i];
+
+    //  copy positions and velocities from C-arrays to RBDL vectors
+    torque_vec_[i] = applied_torque[i];
+    q_vec_[i] = q[i];
+    q_dotvec_[i] = q_dot[i];
+    q_ddotvec_[i] = 0.0;
+  }
+  // calculate dynamics (q_dotdot) with RBDL
+  RigidBodyDynamics::ForwardDynamics(rbdl_model, q_vec_, q_dotvec_, torque_vec_, q_ddotvec_);
+
+  // copy Q_dotdot from RBDL vector type to simple C-arrays
+  for (size_t i = 0; i < NDOF; ++i)
+  {
+    q_dotdot[i] = q_ddotvec_[i];
+  }
+
+  return (0);
+}
+
+int Manip::stateDeriv()
+{
+  int status_code = forwardDynamics();
+  return (status_code);
+}
+
+int Manip::stateInteg()
+{
+  int integration_step;
+  load_state(&q[0], &q[1], &q[2], &q[3], &q[4], &q[5], &q[6], &q_dot[0], &q_dot[1], &q_dot[2], &q_dot[3], &q_dot[4],
+             &q_dot[5], &q_dot[6], NULL);
+  load_deriv(&q_dot[0], &q_dot[1], &q_dot[2], &q_dot[3], &q_dot[4], &q_dot[5], &q_dot[6], &q_dotdot[0], &q_dotdot[1],
+             &q_dotdot[2], &q_dotdot[3], &q_dotdot[4], &q_dotdot[5], &q_dotdot[6], NULL);
+  // integrate
+  integration_step = integrate();
+
+  unload_state(&q[0], &q[1], &q[2], &q[3], &q[4], &q[5], &q[6], &q_dot[0], &q_dot[1], &q_dot[2], &q_dot[3], &q_dot[4],
+               &q_dot[5], &q_dot[6], NULL);
+
+  return (integration_step);
+}

ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.hh

@@ -0,0 +1,97 @@
+// Copyright 2024 Blazej Fiderek (xfiderek)
+// 
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+// 
+//     https://www.apache.org/licenses/LICENSE-2.0
+// 
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#ifndef __MANIPULATOR_HH_
+#define __MANIPULATOR_HH_
+/**************************************************************************
+PURPOSE: (2D Manipulator class definitions including kinematics and control)
+***************************************************************************/
+#define TRICK_NO_MONTE_CARLO
+#define TRICK_NO_MASTERSLAVE
+#define TRICK_NO_INSTRUMENTATION
+
+#include <rbdl/rbdl.h>
+#include <rbdl/rbdl_utils.h>
+#include <rbdl/addons/urdfreader/urdfreader.h>
+#include <cmath>
+#include <iostream>
+#include <new>
+
+class Manip
+{
+public:
+  Manip();
+
+  /**
+   * @brief Calculate Q_dotdot (joint accelerations) given current state and input torques
+   * The dynamics is calculated using Articulated Body Algorithm (ABA) with RBDL library
+   * Friction is modelled with Stribeck model taken from Mathworks docs
+   * https://www.mathworks.com/help/simscape/ref/rotationalfriction.html
+   * @return int status code
+   */
+  int forwardDynamics();
+
+  /**
+   * @brief Calculate state derivative. Calls forwardDynamics().
+   *
+   * @return int status code
+   */
+  int stateDeriv();
+  /**
+   * @brief Propagate state for the current timestamp by integration. Called after stateDeriv()
+   *
+   * @return int status code
+   */
+  int stateInteg();
+
+  /**
+   * @brief Initializes all data structures. Called on startup.
+   *
+   * @return int status code
+   */
+  int defaultData();
+
+  /**
+   * @brief Model encapsulating kinematics and dynamics of Canadarm with RBDL library
+   *
+   */
+  RigidBodyDynamics::Model rbdl_model; /* ** -- class from RBDL that calculates forward dynamics */
+  static const size_t NDOF = 7;        /* ** -- ndof */
+
+  double input_torque[NDOF] = { 0.0 }; /* *i (N.m) input (commanded) torque for each joint */
+
+  double q[NDOF] = { 0.0 };               /* *o rad angle of joints */
+  double q_dot[NDOF] = { 0.0 };           /* *o (rad/s) velocity of joints */
+  double q_dotdot[NDOF] = { 0.0 };        /* *o (rad/s^2) accelerations of joints */
+  double friction_torque[NDOF] = { 0.0 }; /* *o (N.m) Torque comming from friction */
+  double applied_torque[NDOF] = { 0.0 };  /* *o (N.m) final torque applied for each joint */
+
+  // Friction-related parameters. For now we assume same params for every joint
+  // The friction is modelled using Stribeck function
+  // https://www.mathworks.com/help/simscape/ref/rotationalfriction.html
+  double breakaway_friction_torque[NDOF];    /* *i (N.m)  */
+  double coloumb_friction_torque[NDOF];      /* *i (N.m)  */
+  double breakaway_friction_velocity[NDOF];  /* *i (rad/s)  */
+  double coulomb_velocity_threshold[NDOF];   /* *i (rad.s)  */
+  double stribeck_velocity_threshold[NDOF];  /* *i (N.m/rad.s)  */
+  double viscous_friction_coefficient[NDOF]; /* *i (N.m/rad.s)  */
+
+private:
+  RigidBodyDynamics::Math::VectorNd q_vec_;
+  RigidBodyDynamics::Math::VectorNd q_dotvec_;
+  RigidBodyDynamics::Math::VectorNd q_ddotvec_;
+  RigidBodyDynamics::Math::VectorNd torque_vec_;
+};
+#endif