diff --git a/.gitignore b/.gitignore index 81d4a7c..0f8e1e3 100644 --- a/.gitignore +++ b/.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 \ No newline at end of file diff --git a/config/models/breeding_blanket_handling_robot.yaml b/config/models/breeding_blanket_handling_robot.yaml index f5c7720..b332800 100644 --- a/config/models/breeding_blanket_handling_robot.yaml +++ b/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 diff --git a/config/models/breeding_blanket_handling_robot_fixed.yaml b/config/models/breeding_blanket_handling_robot_fixed.yaml new file mode 100644 index 0000000..a8163e5 --- /dev/null +++ b/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 diff --git a/examples/simulation/CMakeLists.txt b/examples/simulation/CMakeLists.txt index 81d34b8..ad166bd 100644 --- a/examples/simulation/CMakeLists.txt +++ b/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) diff --git a/examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp b/examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp new file mode 100644 index 0000000..ff6099d --- /dev/null +++ b/examples/simulation/eurofusion/cpp/eurofusion_cartesian_motion_controller.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(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 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& 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 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(); +} diff --git a/include/sdu_controllers/controllers/operational_space_controller.hpp b/include/sdu_controllers/controllers/operational_space_controller.hpp index dd79cb6..165e212 100644 --- a/include/sdu_controllers/controllers/operational_space_controller.hpp +++ b/include/sdu_controllers/controllers/operational_space_controller.hpp @@ -46,9 +46,15 @@ namespace sdu_controllers::controllers */ void reset() override; + /** + * Set kappa used in the damped least squares. + */ + void set_kappa(double kappa); + private: Eigen::VectorXd y_; Eigen::MatrixXd Kp_, Kd_; + double kappa_; std::shared_ptr robot_model_; }; diff --git a/python/sdu_controllers/__init__.py b/python/sdu_controllers/__init__.py index e386495..e1dae64 100644 --- a/python/sdu_controllers/__init__.py +++ b/python/sdu_controllers/__init__.py @@ -50,27 +50,27 @@ # # return PIDController_(Kp, Ki, Kd, N, dt) -class _PIDController(_original_PIDController): - def __init__(self, Kp, Ki, Kd, N, dt): - args = [Kp, Ki, Kd, N] +# class _PIDController(_original_PIDController): +# def __init__(self, Kp, Ki, Kd, N, dt): +# args = [Kp, Ki, Kd, N] - for i, arg in enumerate(args): - if isinstance(arg, float): - args[i] = _np.array([[arg]]) +# for i, arg in enumerate(args): +# if isinstance(arg, float): +# args[i] = _np.array([[arg]]) - super(_PIDController, self).__init__(*args, dt) +# super(_PIDController, self).__init__(*args, dt) - self.output = _np.zeros((args[0].shape[0],)) +# self.output = _np.zeros((args[0].shape[0],)) - def step(self, q_d, dq_d, u_ff, q, dq): - args = [q_d, dq_d, u_ff, q, dq] +# def step(self, q_d, dq_d, u_ff, q, dq): +# args = [q_d, dq_d, u_ff, q, dq] - for i, arg in enumerate(args): - if isinstance(arg, float): - args[i] = _np.array([arg]) +# for i, arg in enumerate(args): +# if isinstance(arg, float): +# args[i] = _np.array([arg]) - super().step(*args) +# super().step(*args) - self.output = super().get_output() +# self.output = super().get_output() -controllers.PIDController = _PIDController \ No newline at end of file +# controllers.PIDController = _PIDController \ No newline at end of file diff --git a/python/sdu_controllers/_robot_models.cpp b/python/sdu_controllers/_robot_models.cpp index 535c9a6..1f63729 100644 --- a/python/sdu_controllers/_robot_models.cpp +++ b/python/sdu_controllers/_robot_models.cpp @@ -92,7 +92,8 @@ namespace sdu_controllers .def(nb::init<>()) .def(nb::init()) .def(nb::init()) - .def(nb::init()); + .def(nb::init()) + .def("set_tcp_mass", &models::BreedingBlanketHandlingRobotModel::set_tcp_mass); return m_models; } } // namespace sdu_controllers \ No newline at end of file diff --git a/simulink/library/admittance_controller.m b/simulink/library/+cartesian_space_control/admittance_controller.m similarity index 92% rename from simulink/library/admittance_controller.m rename to simulink/library/+cartesian_space_control/admittance_controller.m index 7cbd5c2..54abd97 100644 --- a/simulink/library/admittance_controller.m +++ b/simulink/library/+cartesian_space_control/admittance_controller.m @@ -40,10 +40,11 @@ function setupImpl(obj) function [x_c, quat_c] = stepImpl(obj, input_force, input_torque, ... x_desired, quat_desired) - % const Eigen::Vector3d &input_force, - % const Eigen::Vector3d &input_torque, - % const Eigen::Vector3d &x_desired, - % const Eigen::Vector4d &quat_desired + + input_force = reshape(input_force, 1, 3); + input_torque = reshape(input_torque, 1, 3); + x_desired = reshape(x_desired, 1, 3); + quat_desired = reshape(quat_desired, 1, 4); obj.adm_contr.step(input_force, input_torque, ... x_desired, quat_desired); diff --git a/simulink/library/+cartesian_space_control/force_control_inner_velocity_loop.m b/simulink/library/+cartesian_space_control/force_control_inner_velocity_loop.m index 09f0162..d7df881 100644 --- a/simulink/library/+cartesian_space_control/force_control_inner_velocity_loop.m +++ b/simulink/library/+cartesian_space_control/force_control_inner_velocity_loop.m @@ -31,7 +31,7 @@ function setupImpl(obj) % Perform one-time calculations, such as computing constants switch obj.RobotType case obj.all_robot_types(1) - obj.robot_model = obj.sdu_controllers.models.BreedingBlanketHandlingRobotModel(); + obj.robot_model = obj.sdu_controllers.models.ParameterRobotModel("/home/madla/Documents/sdu_controllers/config/models/breeding_blanket_handling_robot.yaml"); case obj.all_robot_types(2) obj.robot_model = obj.sdu_controllers.models.URRobotModel(... @@ -50,6 +50,11 @@ function setupImpl(obj) end function [y] = stepImpl(obj, f_d, f_e, q, dq) + f_d = reshape(f_d, 1, 6); + f_e = reshape(f_e, 1, 6); + q = reshape(q, 1, obj.links); + dq = reshape(dq, 1, obj.links); + obj.f_contr.step(f_d, f_e, q, dq); y = double(obj.f_contr.get_output()); diff --git a/simulink/library/+cartesian_space_control/operational_space_controller.m b/simulink/library/+cartesian_space_control/operational_space_controller.m index f68be8b..b1937f6 100644 --- a/simulink/library/+cartesian_space_control/operational_space_controller.m +++ b/simulink/library/+cartesian_space_control/operational_space_controller.m @@ -34,7 +34,7 @@ function setupImpl(obj) % Perform one-time calculations, such as computing constants switch obj.RobotType case obj.all_robot_types(1) - obj.robot_model = obj.sdu_controllers.models.BreedingBlanketHandlingRobotModel(); + obj.robot_model = obj.sdu_controllers.models.ParameterRobotModel("/home/madla/Documents/sdu_controllers/config/models/breeding_blanket_handling_robot.yaml"); case obj.all_robot_types(2) obj.robot_model = obj.sdu_controllers.models.URRobotModel(... @@ -53,6 +53,12 @@ function setupImpl(obj) end function [y] = stepImpl(obj, x_d, dx_d, ddx_d, q, dq) + x_d = reshape(x_d, 1, 6); + dx_d = reshape(dx_d, 1, 6); + ddx_d = reshape(ddx_d, 1, 6); + q = reshape(q, 1, obj.links); + dq = reshape(dq, 1, obj.links); + obj.os_contr.step(x_d, dx_d, ddx_d, q, dq); y = double(obj.os_contr.get_output()); diff --git a/simulink/library/coriolis_matrix.m b/simulink/library/+dynamics/coriolis_matrix.m similarity index 100% rename from simulink/library/coriolis_matrix.m rename to simulink/library/+dynamics/coriolis_matrix.m diff --git a/simulink/library/forward_dynamics.m b/simulink/library/+dynamics/forward_dynamics.m similarity index 100% rename from simulink/library/forward_dynamics.m rename to simulink/library/+dynamics/forward_dynamics.m diff --git a/simulink/library/gravity_torques.m b/simulink/library/+dynamics/gravity_torques.m similarity index 100% rename from simulink/library/gravity_torques.m rename to simulink/library/+dynamics/gravity_torques.m diff --git a/simulink/library/inertia_matrix.m b/simulink/library/+dynamics/inertia_matrix.m similarity index 100% rename from simulink/library/inertia_matrix.m rename to simulink/library/+dynamics/inertia_matrix.m diff --git a/simulink/library/inverse_dynamics.m b/simulink/library/+dynamics/inverse_dynamics.m similarity index 100% rename from simulink/library/inverse_dynamics.m rename to simulink/library/+dynamics/inverse_dynamics.m diff --git a/simulink/library/+dynamics/set_tcp_mass.m b/simulink/library/+dynamics/set_tcp_mass.m index 02204cd..0aa893f 100644 --- a/simulink/library/+dynamics/set_tcp_mass.m +++ b/simulink/library/+dynamics/set_tcp_mass.m @@ -23,7 +23,10 @@ function setupImpl(obj) end function [] = stepImpl(obj, mass, com, inertia) - obj.robot_model.set_tcp_mass(mass, com, inertia); + + com = reshape(com, 3, 1); + + obj.robot_model.set_tcp_mass(mass, com', inertia); end function [] = isOutputFixedSizeImpl(~) diff --git a/simulink/library/pid_controller.m b/simulink/library/+joint_space_control/pid_controller.m similarity index 91% rename from simulink/library/pid_controller.m rename to simulink/library/+joint_space_control/pid_controller.m index 49bb275..697c563 100644 --- a/simulink/library/pid_controller.m +++ b/simulink/library/+joint_space_control/pid_controller.m @@ -13,6 +13,10 @@ % num_states Number of States num_states = 6 + + % + u_min = -100 + u_max = 100 % sample_time Sample Time sample_time = 1/500. @@ -24,6 +28,8 @@ Ki_mat Kd_mat N_mat + u_min_mat + u_max_mat pid_contr sdu_controllers end @@ -34,11 +40,14 @@ function setupImpl(obj) obj.Ki_mat = obj.Ki * eye(obj.num_states); obj.Kd_mat = obj.Kd * eye(obj.num_states); obj.N_mat = obj.N * eye(obj.num_states); + obj.u_min_mat = ones(1,obj.num_states) * obj.u_min; + obj.u_max_mat = ones(1,obj.num_states) * obj.u_max; + - % + obj.sdu_controllers = py.importlib.import_module('sdu_controllers'); obj.pid_contr = obj.sdu_controllers.controllers.PIDController(obj.Kp_mat, ... - obj.Ki_mat, obj.Kd_mat, obj.N_mat, obj.sample_time); + obj.Ki_mat, obj.Kd_mat, obj.N_mat, obj.sample_time,obj.u_min_mat,obj.u_max_mat ); end % function sts = getSampleTimeImpl(obj) diff --git a/simulink/library/+kinematics/forward_kinematics.m b/simulink/library/+kinematics/forward_kinematics.m index 52ebebb..e6b1ada 100644 --- a/simulink/library/+kinematics/forward_kinematics.m +++ b/simulink/library/+kinematics/forward_kinematics.m @@ -17,6 +17,7 @@ properties (Access = private) robot_model links + fk_solver all_robot_types = ["BB Handler", "UR3e", "UR5e"]; @@ -30,8 +31,8 @@ function setupImpl(obj) % Perform one-time calculations, such as computing constants switch obj.RobotType case obj.all_robot_types(1) - obj.robot_model = obj.sdu_controllers.models.BreedingBlanketHandlingRobotModel(); - obj.links = 7; + obj.robot_model = obj.sdu_controllers.models.ParameterRobotModel("/home/madla/Documents/sdu_controllers/config/models/breeding_blanket_handling_robot.yaml"); + obj.links = 6; case obj.all_robot_types(2) obj.robot_model = obj.sdu_controllers.models.URRobotModel(... @@ -46,6 +47,8 @@ function setupImpl(obj) obj.links = 6; end + obj.fk_solver = obj.robot_model.get_fk_solver(); + % obj.links = double(obj.robot_model.get_dof()); % disp(obj.links) end @@ -53,7 +56,7 @@ function setupImpl(obj) function [T] = stepImpl(obj, q) % Implement algorithm. Calculate y as a function of input u and % internal states. - T = obj.sdu_controllers.kinematics.forward_kinematics(q, obj.robot_model); + T = obj.fk_solver.forward_kinematics(q); T = double(T); end diff --git a/simulink/library/sdu_controllers.slx b/simulink/library/sdu_controllers.slx index 6569512..6a1e722 100644 Binary files a/simulink/library/sdu_controllers.slx and b/simulink/library/sdu_controllers.slx differ diff --git a/simulink/tests/admittance_controller_test.slx b/simulink/tests/admittance_controller_test.slx index 8e1851b..ca926e0 100644 Binary files a/simulink/tests/admittance_controller_test.slx and b/simulink/tests/admittance_controller_test.slx differ diff --git a/simulink/tests/dyn_compononents_test.slx b/simulink/tests/dyn_compononents_test.slx index d133b95..49d9b7c 100644 Binary files a/simulink/tests/dyn_compononents_test.slx and b/simulink/tests/dyn_compononents_test.slx differ diff --git a/simulink/tests/eurofusion_force_control.slx b/simulink/tests/eurofusion_force_control.slx index 0354008..0984cf1 100644 Binary files a/simulink/tests/eurofusion_force_control.slx and b/simulink/tests/eurofusion_force_control.slx differ diff --git a/simulink/tests/init.m b/simulink/tests/init.m index 2db6547..220c21d 100644 --- a/simulink/tests/init.m +++ b/simulink/tests/init.m @@ -10,8 +10,8 @@ addpath("../library/") % Create robot model -sdu_controllers_lib = py.importlib.import_module('sdu_controllers') +sdu_controllers_lib = py.importlib.import_module('sdu_controllers'); bbrobot = sdu_controllers_lib.models.BreedingBlanketHandlingRobotModel(); ur3e = sdu_controllers_lib.models.URRobotModel( ... - sdu_controllers_lib.models.RobotType(0)); \ No newline at end of file + sdu_controllers_lib.models.RobotType(0)); diff --git a/src/controllers/operational_space_controller.cpp b/src/controllers/operational_space_controller.cpp index df1b2a1..d0a225f 100644 --- a/src/controllers/operational_space_controller.cpp +++ b/src/controllers/operational_space_controller.cpp @@ -2,6 +2,7 @@ #include #include #include +#include using namespace Eigen; @@ -15,6 +16,7 @@ namespace sdu_controllers::controllers Kd_(std::move(Kd)), robot_model_(std::move(robot_model)) { + kappa_ = 0; } void OperationalSpaceController::step( @@ -44,11 +46,15 @@ namespace sdu_controllers::controllers VectorXd dx_tilde = dx_d - dx_e; - // Eq. (8.114) from page 348, Robotics: Modelling, Planning and Control: - // TODO: This will only work for a robot with six joints, since you cannot take the inverse - // of a 6x7 sized Jacobian. - y_ = J_A.lu().solve(ddx_d + Kd_ * dx_tilde + Kp_ * x_tilde - Jdot_A * dq); + Eigen::MatrixXd kappaI; + kappaI.setIdentity(6, 6); + kappaI *= kappa_*kappa_; // todo: should be user configurable + + // The following implements damped least-squares, see eq. (3.59) + y_ = J_A.transpose() * (J_A * J_A.transpose() + kappaI).fullPivHouseholderQr().solve( + ddx_d + Kd_ * dx_tilde + Kp_ * x_tilde - Jdot_A * dq + ); } void OperationalSpaceController::reset() @@ -56,6 +62,7 @@ namespace sdu_controllers::controllers Kp_.setZero(); Kd_.setZero(); y_.setZero(); + kappa_ = 0; } VectorXd OperationalSpaceController::get_output() @@ -63,4 +70,9 @@ namespace sdu_controllers::controllers return y_; } + void OperationalSpaceController::set_kappa(double kappa) + { + kappa_ = kappa; + } + } // namespace sdu_controllers::controllers