diff --git a/ros_trick/trick_src/SIM_trick_canadarm/RUN_2DPlanar/input.py b/ros_trick/trick_src/SIM_trick_canadarm/RUN_2DPlanar/input.py new file mode 100644 index 00000000..9af246aa --- /dev/null +++ b/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) diff --git a/ros_trick/trick_src/SIM_trick_canadarm/S_define b/ros_trick/trick_src/SIM_trick_canadarm/S_define new file mode 100644 index 00000000..aa74ebd5 --- /dev/null +++ b/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); +} diff --git a/ros_trick/trick_src/SIM_trick_canadarm/S_overrides.mk b/ros_trick/trick_src/SIM_trick_canadarm/S_overrides.mk new file mode 100644 index 00000000..7d855259 --- /dev/null +++ b/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 + diff --git a/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.cc b/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.cc new file mode 100644 index 00000000..3adbb88d --- /dev/null +++ b/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); +} \ No newline at end of file diff --git a/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.hh b/ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.hh new file mode 100644 index 00000000..8f283636 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include + +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