Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • ipk_aut/converging/impedance_controller
1 result
Show changes
Commits on Source (101)
1 additional commits have been omitted to prevent performance issues.
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 1018 additions and 5796 deletions
.gitignore
View file @ ec1af206
.vscode/
build/
assets/data/*.asv
# assets/data/*.csv
fr3_impedance_controller_sim/build/
fr3_ros1_mujoco_interface/build/
fr3_ps3_joy_publisher/build/
assets/data/matlab/*.asv
assets/data/*.csv
# assets/data/*.bag
CMakeLists.txt deleted 100644 → 0
View file @ bf476964
################################################################################
## Set minimum required version of cmake, project name and compile options
################################################################################
cmake_minimum_required(VERSION 3.10)
project(dob_impedance_controller)
# Set C++ standard
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED True)
################################################################################
## Find required CMake packages : pinocchio, casadi, catkin, ROS-related ...
################################################################################
find_package(pinocchio REQUIRED)
find_package(casadi REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roscpp
rosbag
rostime
urdf
franka_msgs
geometry_msgs
)
################################################################################
## Find source and header files to create executable binary file
################################################################################
file(GLOB_RECURSE SOURCES
${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp
${CMAKE_CURRENT_SOURCE_DIR}/common/controller/src/*.cpp
${CMAKE_CURRENT_SOURCE_DIR}/common/utilities/src/*.cpp
${CMAKE_CURRENT_SOURCE_DIR}/common/robot/src/*.cpp
${CMAKE_CURRENT_SOURCE_DIR}/common/math/src/*.cpp
)
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/include/dob_impedance_controller # ! check this out
${CMAKE_CURRENT_SOURCE_DIR}/common/controller/include
${CMAKE_CURRENT_SOURCE_DIR}/common/utilities/include
${CMAKE_CURRENT_SOURCE_DIR}/common/robot/include
${CMAKE_CURRENT_SOURCE_DIR}/common/math/include
${catkin_INCLUDE_DIRS}
)
################################################################################
## Define executable, Add source files & Link libraries
################################################################################
add_executable(${PROJECT_NAME}_node ${SOURCES})
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
pinocchio::pinocchio
casadi
)
README.md
View file @ ec1af206
# DOB-based Impedance Controller Package
This branch is a ROS1 node for a DOB-based impedance controller.
# impedance_controller
This is a repo has several ROS1 sub-packages that are for Franka Research 3 (FR3) robot control
both in simulation (MuJoCo) and real hardware interface that is originated from main branch.
Other branches, e.g. 5-observer-implementation-real, 6-simulator-implementation, will be collected
in this branch as a separate ROS1 package. After the collection is done, this branch will be merged
to the main branch s.t. impedance_controller repo to be a meta-package that includes controller,
simulator, and other features.
>[!note] This package is **under valdiation and debugging**. After done with validation, this branch
> will be merged into main branch s.t. `impedance_controller` (meta)package to include this package
> as a (sub)package
for example,
```txt
.impedance_controller
|- assets
|- data
|- model
|- common
|- controller
|- math
|- motion
|- robot
|- utilities
|- dob_impedance_controller
|- ros1_mujoco_interface
|- test
|- .clang-format
|- .gitignore
|- flake.lock
|- flake.nix
|- README.md
```
## Repository Structure
## Repository Tree Description
```txt
.impedance_controller
|- assets
|- data
|- matlab
|- rosbag
|- data.csv
|- model
|- meshes
|- fr3_hand.urdf
|- fr3.urdf
|- common
|- controller
|- include
|- DisturbanceObserver.hpp
|- SlidingModeObserver.hpp
|- src
|- DisturbanceObserver.cpp
|- SlidingModeObserver.cpp
|- math
|- include
|- MathUtilities.hpp
|- pseudo_inversion.h
|- src
|- motion
|- include
|- sinusoidal_profile.hpp
|- src
|- sinusoidal_profile.cpp
|- robot
|- dynamics_func.hpp
|- kinematics_func.hpp
|- utilities
|- include/dob_impedance_controller
|- dob_impedance_controller.hpp
|- src
|- dob_impedance_controller_node.cpp
|- dob_impedance_controller.cpp
|- include
|- cppTypes.hpp
|- save_data.hpp
|- src
|- save_data.cpp
|- pkg_impedance_controller
|- include
|- impedance_controller
|- impedance_controller.h
|- src
|- impedance_controller.cpp
|- CMakeLists.txt
|- package.xml
|- pkg_offline_observer
|- include
|- offline_observer
|- offline_observer.hpp
|- src
|- offline_observer_node.cpp
|- offline_observer.cpp
|- CMakeLists.txt
|- package.xml
|- pkg_pose_motion_publisher
|- include
|- pose_motion_publisher
|- pose_motion_publisher.hpp
|- src
|- pose_motion_publisher_node.cpp
|- pose_motion_publisher.cpp
|- CMakeLists.txt
|- package.xml
|- pkg_test_impedance_controller
|- include
|- test_impedance_controller
|- test_impedance_controller.hpp
|- src
|- test_impedance_controller_node.cpp
|- test_impedance_controller.cpp
|- CMakeLists.txt
|- package.xml
|- test
|- .clang-format
|- .gitignore
|- CMakeLists.txt
|- flake.lock
|- flake.nix
|- package.xml
|- README.md
```
### TODO
- [ ] Implement MOB and compare joint acceleration & torque
- [ ] Modulate (Joint) Inertia matrix's diagonal term to amplify inertia of joint5-7
- [ ] Check whether applying diagonal operational inertia affects disturbance decoupling.
- [ ] Modulize controller s.t. it can be utilized both in real HW interface and ros1 simulator interface
- [ ] Tune SOSML's parameters
- [ ] Implement dynamics_reconfigure to tune parameters
- [ ] (Optional) Run data logger as a different thread
- [ ] (Optional) Add Homing feature s.t. when trajectory is recorded using `rosbag`, and replayed, it can follow the same trajectory.
- [ ] Code review -> check errors & debug
- [ ] Check the control flow of TFDOB & SOSML
- [ ] Add Homing feature s.t. when trajectory is recorded using `rosbag`, and replayed, it can follow the same trajectory.
## Getting started
To make it easy for you to get started with GitLab, here's a list of recommended next steps.
Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)!
## Add your files
- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files
- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command:
```
cd existing_repo
git remote add origin https://gitlab.cc-asp.fraunhofer.de/ipk_aut/converging/impedance_controller.git
git branch -M main
git push -uf origin main
```
## Integrate with your tools
- [ ] [Set up project integrations](https://gitlab.cc-asp.fraunhofer.de/ipk_aut/converging/impedance_controller/-/settings/integrations)
## Collaborate with your team
- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/)
- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html)
- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically)
- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/)
- [ ] [Set auto-merge](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html)
## Test and Deploy
Use the built-in continuous integration in GitLab.
- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html)
- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing (SAST)](https://docs.gitlab.com/ee/user/application_security/sast/)
- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html)
- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/)
- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html)
***
# Editing this README
When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thanks to [makeareadme.com](https://www.makeareadme.com/) for this template.
## Suggestions for a good README
Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information.
## Name
Choose a self-explaining name for your project.
## Description
Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors.
## Badges
On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge.
## Visuals
Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method.
## Installation
Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection.
## Usage
Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README.
## Support
Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc.
## Roadmap
If you have ideas for releases in the future, it is a good idea to list them in the README.
## Contributing
State if you are open to contributions and what your requirements are for accepting them.
For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self.
You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser.
## Authors and acknowledgment
Show your appreciation to those who have contributed to the project.
## License
For open source projects, say how it is licensed.
## Project status
If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers.
assets/data/data.csv deleted 100644 → 0
View file @ bf476964
This diff is collapsed.
assets/data/matlab/data_analysis_real.m 0 → 100644
View file @ ec1af206
%% load data
clc; close all; clear;
bag = rosbag('../rosbag/250124_ft_contact_03.bag');
% disp(bag.AvailableTopics.Row);
% Load FR3 states
% fr3_bag = select(bag, 'Topic', '/franka_state_controller/franka_states/O_F_ext_hat_K');
% disp(fr3_bag.NumMessages);
% fr3_msgs = readMessages(fr3_bag);
% fr3_msgs_num = length(fr3_msgs);
% Load DOB estimation
dob_bag = select(bag, 'Topic', '/cartesian_impedance_example_controller/estimated_disturb');
dob_msgs = readMessages(dob_bag);
dob_msgs_num = length(dob_msgs);
% Load FT sensor data
ft_bag = select(bag, 'Topic', '/external_ft_sensor');
ft_msgs = readMessages(ft_bag);
ft_msgs_num = length(ft_msgs);
%%
time_ft = zeros(ft_msgs_num, 1);
ft_effort_cell = cell(ft_msgs_num, 1);
% FT sensor data processing
for i = 1:ft_msgs_num
time_ft(i) = ft_msgs{i}.Header.Stamp.Sec + ft_msgs{i}.Header.Stamp.Nsec * 1e-9;
ft_effort_cell{i} = ft_msgs{i}.Effort;
end
time_ft = time_ft - time_ft(1);
ft_effort_matrix = cell2mat(ft_effort_cell');
ft_effort_offset = ft_effort_matrix(:,1);
ft_effort_matrix = ft_effort_matrix';
% DOB data processing
time_dob = zeros(dob_msgs_num, 1);
for i = 1:dob_msgs_num
time_dob(i) =
% time_fr3 = zeros(fr3_msgs_num, 1);
%%
lw = 1.2;
font = 'Times New Roman';
fs_lgd = 14;
fs_lbl = 22;
fs_axis = 14;
fs_title = 20;
%% Joint acceleration
figure();
fig_ddq = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\ddot{q}_1$', '$\ddot{q}_2$', '$\ddot{q}_3$', '$\ddot{q}_4$', '$\ddot{q}_5$', ...
'$\ddot{q}_6$', '$\ddot{q}_7$',};
plt_indices = 1:7;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, ddq_eul(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'k');
plt1 = plot(t, ddq_hat_sosml(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 2 * lw, 'Color', 'b');
% plt1.Color(4) = 0.7;
% plt2 = plot(t, ddq_mj(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
% plt2.Color(4) = 0.7;
% plot(t, ddq_hat_mob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
plot(t, ddq_hat_mob_filtered(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
% plot(t_dob, ddq_hat_mob_filtered_dob(:,plt_indices(i)), 'LineStyle', '-.', 'LineWidth', lw, 'Color', 'b');
xlim([t_start, t_end]);
% ylim([-1 1]);
ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
% if i == 7
% ylim([-1 1]);
% end
if i == 1
legend('Time-derivative (w/ LPF) of $\dot{q}$', 'Estimation using SOSML', 'Estimation using MOB (w/ LPF)', ...
'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_ddq, 'Joint Acceleration $[rad/s^2]$', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_ddq, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Task acceleration
figure();
fig_ddx = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\ddot{p}_x$', '$\ddot{p}_y$', '$\ddot{p}_z$', '$\dot\omega_{x}$', ...
'$\dot\omega_{y}$', '$\dot\omega_{z}$'};
plt_indices = 1:6;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, ee_acc_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t, ee_acc_eul(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
plot(t, ee_acc_hat_sosml(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
% plot(t, ddx_mj(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1 * lw, 'Color', 'g');
xlim([t_start, t_end]);
ylabel(ytitles(i), 'Interpreter', 'latex', 'FontSize', fs_lbl);
% if i == 1 || i == 2 || i == 3
% ylim([-1 1]);
% elseif i == 4 || i == 5 || i == 6
% ylim([-2 2]);
% end
if i == 3
legend('Euler method', 'Estimated by SOSML', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_ddx, 'Task Acceleration', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_ddx, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Pose Tracking Performance
figure();
fig_pose = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$p_{ee} \ [m]$', '$\psi_{ee} \ [rad]$'};
plt_indices = 1:3;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, ee_pos_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t, ee_pos_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', lw, 'Color', 'r');
plot(t_dob, ee_pos_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
% plot(t, fr_ee_pos_mes(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
xlim([t_start, t_end]);
if i == 1
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 1
legend('Desired', 'Impedance control', 'Impedance control + DOB' , 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, ee_rpy_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t, ee_rpy_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', lw, 'Color', 'r');
plot(t_dob, ee_rpy_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
% plot(t, fr_ee_rpy_mes(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
xlim([t_start, t_end]);
if i == 1
ylabel(ytitles{mod(i-1, 2) + 1 + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
end
title(fig_pose, 'Pose Tracking', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_pose, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Twist Tracking Performance
figure();
fig_twist = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\dot{p}_{ee} \ [m/s]$', '$\omega_{ee} \ [rad/s]$'};
plt_indices = 1:6;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, ee_vel_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t, ee_vel_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 1.5 * lw, 'Color', 'r');
plot(t_dob, ee_vel_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'b');
% plot(t, ee_vel_mj(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
xlim([t_start, t_end]);
if i == 1 || i == 4
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 1
legend('Desired', 'Impedance control', 'Impedance control + DOB' , 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_twist, 'Twist Tracking', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_twist, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Estimated disturbance
figure();
fig_dist = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\hat{F}_{ext} \ [N]$', '$\hat{T}_{ext} \ [N/m]$'};
plt_indices = 1:6;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, F_dist_hat_dob(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 2*lw, 'Color', 'r');
plot(t, F_dist_hat_mob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1 * lw, 'Color', 'b');
% plot(t, mj_ee_frc(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 1 * lw, 'Color', 'b');
% plot(t, fr_0_F_ext_hat_K(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
% plot(t, fr_F_dist_hat_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
plot(t, -fr_F_ext_hat_filtered(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
xlim([t_start, t_end]);
if i == 1 || i == 2 || i == 3
ylim([-20 20]);
end
if i == 4 || i == 5 || i == 6
ylim([-4 4]);
end
if i == 1 || i == 4 % 첫 번째 및 네 번째 플롯에 y축 레이블 추가
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 1
legend('Estimated disturb by DOB', 'Estimated disturb by MOB', 'Estimated disturb by Franka state', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
% legend('DOB', 'F_dist_hat_dob (FR3)', 'F_ext_hat_filtered', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_dist, 'Task Disturbance Estimation', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_dist, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Joint torque disturbance estiamtion
figure();
fig_ddq = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\tau_1$', '$\tau_2$', '$\tau_3$', '$\tau_4$', '$\tau_5$', '$\tau_6$', '$\tau_7$',};
plt_indices = 1:7;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, tau_dist_hat_mob(:,plt_indices(i)), 'LineStyle', '--', 'LineWidth', 1.5 * lw, 'Color', 'k');
plot(t, tau_dist_hat_dob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
plot(t, mj_tau_dist_hat(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
% plot(t, tau_c(:,plt_indices(i)) + tau_g(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
% plot(t, tau_c(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
xlim([t_start, t_end]);
% ylim([-5 5]);
ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
if i == 1
legend('MOB estimation', 'DOB estimation', 'MuJoCo', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_ddq, 'Joint Acceleration $[rad/s^2]$', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_ddq, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Comparison w/ FT sensor data
figure();
fig_dist = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\hat{F}_{ext} \ [N]$', '$\hat{T}_{ext} \ [N/m]$'};
plt_indices = 1:6;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, fr_F_dist_hat_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
plot(t, -fr_F_ext_hat_filtered(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', 2 * lw, 'Color', 'r');
% plot(t, -fr_0_F_ext_hat_K(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', lw, 'Color', 'g');
plot(time_ft, ft_effort_matrix(:, plt_indices(i)) - ft_effort_offset(i), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
xlim([t_start, t_end]);
if i <= 3
ylim([-25 25]);
else
ylim([-4 4]);
end
t_patch = 15;
x_patch = [t_patch, max(t), max(t), t_patch];
y_limits = ylim; % 현재 y축 범위 가져오기
y_patch = [y_limits(1), y_limits(1), y_limits(2), y_limits(2)];
fill(x_patch, y_patch, 'cyan', 'FaceAlpha', 0.1, 'EdgeColor', 'none');
if i == 1 || i == 4
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 1
legend('DOB Estimation', 'Franka Estimation', 'FT Sensor', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_dist, 'Task Disturbance Estimation', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_dist, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
assets/data/matlab/plot_data.m
View file @ ec1af206
......@@ -7,11 +7,11 @@
%%% ACCEL ESTIMATION
% 7-dof joint accel
% - euler method => ddq_euler
% - SOSML estimation => ddq_hat
% - SOSML estimation => ddq_hat_sosml
% - mj data => ddq_mj
% 6-dof task accel
% - euler method => ddx_euler
% - SOSML estimation => ddx_hat
% - SOSML estimation => ee_acc_hat_sosml
% - mj data => ddx_mj
%%% DISTURBANCE ESTIMATION
......@@ -30,15 +30,14 @@
%% load data
clc; close all; clear;
filename = 'data.csv';
% filename = 'data_joy.csv';
% filename = 'data_dist.csv';
filename = '../data.csv';
% filename = '../data_sim_joy_dob.csv';
% filename = '../data_sim_free_imp.csv';
% filename = '../data_real_static_disturb.csv';
data_table = readtable(filename);
data_array = table2array(data_table);
% t, ee_pos_des, ee_pos_mes, ee_rpy_des, ee_rpy_mes, ee_vel_des, ee_vel_mes, ee_vel_mj, ddq_eul, ddq_hat,
% ddq_mj, ddx_des, ddx_eul, ddx_hat, F_dist_dob, F_dist_mj
vec_size = [1, 3, 3, 3, 3, 6, 6, 6, 7, 7, 7, 6, 6, 6, 6, 6, 6];
vec_size = [1, 7, 7, 7, 7];
start_idx = [1, ];
for i = 1:length(vec_size) - 1
......@@ -52,22 +51,41 @@ for i = 1:length(vec_size)
end
t = data_cell{1};
ee_pos_des = data_cell{2};
ee_pos_mes = data_cell{3};
ee_rpy_des = data_cell{4};
ee_rpy_mes = data_cell{5};
ee_vel_des = data_cell{6};
ee_vel_mes = data_cell{7};
ee_vel_mj = data_cell{8};
ddq_eul = data_cell{9};
ddq_hat = data_cell{10};
ddq_mj = data_cell{11};
ddx_des = data_cell{12};
ddx_eul = data_cell{13};
ddx_hat = data_cell{14};
ddx_mj = data_cell{15};
F_dist_dob = data_cell{16};
F_dist_mj = data_cell{17};
ddq_mj = data_cell{2};
ddq_eul = data_cell{3};
ddq_mob = data_cell{4};
ddq_sosml = data_cell{5};
% fr_q = data_cell{2};
% fr_dq = data_cell{3};
% fr_tau_des = data_cell{4};
% fr_ee_pos_mes = data_cell{5};
% fr_ee_rpy_mes = data_cell{6};
% fr_tau_ext_hat_filtered = data_cell{7};
% fr_F_ext_hat_filtered = data_cell{8};
% fr_0_F_ext_hat_K = data_cell{9};
% fr_F_dist_hat_dob = data_cell{10};
%
% ddq_eul = data_cell{11};
% ddq_hat_sosml = data_cell{12};
% ddq_hat_mob = data_cell{13};
% tau_des = data_cell{14};
% ee_pos_des = data_cell{15};
% ee_pos_mes = data_cell{16};
% ee_rpy_des = data_cell{17};
% ee_rpy_mes = data_cell{18};
% ee_vel_des = data_cell{19};
% ee_vel_mes = data_cell{20};
% ee_acc_des = data_cell{21};
% ee_acc_eul = data_cell{22};
% ee_acc_hat_sosml = data_cell{23};
% F_dist_hat_dob = data_cell{24};
% tau_dist_hat_mob = data_cell{25};
% tau_dist_hat_dob = data_cell{26};
% mj_tau_dist_hat = data_cell{27};
% ddq_hat_mob_filtered = data_cell{28};
% mj_ee_frc = data_cell{29};
% F_dist_hat_mob = data_cell{30};
t_start = t(1);
t_end = t(end);
......@@ -90,19 +108,22 @@ for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, ddq_eul(:,plt_indices(i)), 'LineStyle', '--', 'LineWidth', 1.5 * lw, 'Color', 'r');
% plt1 = plot(t, ddq_hat(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 2 * lw, 'Color', 'b');
plt2 = plot(t, ddq_mj(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
% plt1.Color(4) = 0.7;
plt2.Color(4) = 0.7;
plot(t, ddq_mj(:,plt_indices(i)), 'LineStyle', '--', 'LineWidth', 1.5 * lw, 'Color', 'k');
plot(t, ddq_eul(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'r');
plot(t, ddq_mob(:,plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 2 * lw, 'Color', 'b');
plot(t, ddq_sosml(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 2 * lw, 'Color', 'g');
% plt2.Color(4) = 0.7;
xlim([t_start, t_end]);
% ylim([-5 5]);
ylim([-5 5]);
ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
% if i == 7
% ylim([-1 1]);
% end
if i == 1
legend('Time-derivative of $\dot{q}$ w/ Euler method', 'Estimation from SOSML', ...
'MuJoCo qacc data', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
legend('MuJoCo', 'Euler method', 'MOB', 'SOSML', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
......@@ -120,18 +141,18 @@ for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, ddx_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t, ddx_eul(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
% plot(t, ddx_hat(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
plot(t, ddx_mj(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1 * lw, 'Color', 'g');
plot(t, ee_acc_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t, ee_acc_eul(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
plot(t, ee_acc_hat_sosml(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
% plot(t, ddx_mj(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1 * lw, 'Color', 'g');
xlim([t_start, t_end]);
ylabel(ytitles(i), 'Interpreter', 'latex', 'FontSize', fs_lbl);
if i == 1 || i == 2 || i == 3
ylim([-1 1]);
elseif i == 4 || i == 5 || i == 6
ylim([-2 2]);
end
% if i == 1 || i == 2 || i == 3
% ylim([-1 1]);
% elseif i == 4 || i == 5 || i == 6
% ylim([-2 2]);
% end
if i == 3
legend('Euler method', 'Estimated by SOSML', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
......@@ -144,7 +165,7 @@ xlabel(fig_ddx, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontN
%% Pose Tracking Performance
figure();
fig_pose = tiledlayout(2, 3, "Padding", "compact", "TileSpacing", "compact");
fig_pose = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$p_{ee} \ [m]$', '$\psi_{ee} \ [rad]$'};
plt_indices = 1:3;
......@@ -153,15 +174,17 @@ for i = 1:length(plt_indices)
hold on;
grid;
plot(t, ee_pos_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t, ee_pos_mes(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
plot(t, ee_pos_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', lw, 'Color', 'r');
plot(t_dob, ee_pos_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
% plot(t, fr_ee_pos_mes(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
xlim([t_start, t_end]);
if i == 1
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 3
legend('Desired', 'Actual', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
if i == 1
legend('Desired', 'Impedance control', 'Impedance control + DOB' , 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
......@@ -170,7 +193,9 @@ for i = 1:length(plt_indices)
hold on;
grid;
plot(t, ee_rpy_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t, ee_rpy_mes(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
plot(t, ee_rpy_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', lw, 'Color', 'r');
plot(t_dob, ee_rpy_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
% plot(t, fr_ee_rpy_mes(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
xlim([t_start, t_end]);
if i == 1
......@@ -183,7 +208,7 @@ xlabel(fig_pose, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'Font
%% Twist Tracking Performance
figure();
fig_twist = tiledlayout(2, 3, "Padding", "compact", "TileSpacing", "compact");
fig_twist = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\dot{p}_{ee} \ [m/s]$', '$\omega_{ee} \ [rad/s]$'};
plt_indices = 1:6;
......@@ -193,15 +218,16 @@ for i = 1:length(plt_indices)
grid;
plot(t, ee_vel_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t, ee_vel_mes(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 1.5 * lw, 'Color', 'r');
plot(t, ee_vel_mj(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
plot(t_dob, ee_vel_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'b');
% plot(t, ee_vel_mj(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
xlim([t_start, t_end]);
if i == 1 || i == 4
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 3
legend('Desired', 'Actual', 'MuJoCo Data' , 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
if i == 1
legend('Desired', 'Impedance control', 'Impedance control + DOB' , 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
......@@ -210,7 +236,7 @@ xlabel(fig_twist, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'Fon
%% Estimated disturbance
figure();
fig_dist = tiledlayout(2, 3, "Padding", "compact", "TileSpacing", "compact");
fig_dist = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\hat{F}_{ext} \ [N]$', '$\hat{T}_{ext} \ [N/m]$'};
plt_indices = 1:6;
......@@ -218,19 +244,98 @@ for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, F_dist_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
plot(t, F_dist_mj(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 1 * lw, 'Color', 'b');
plot(t, F_dist_hat_dob(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 2*lw, 'Color', 'r');
plot(t, F_dist_hat_mob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1 * lw, 'Color', 'b');
% plot(t, mj_ee_frc(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 1 * lw, 'Color', 'b');
% plot(t, fr_0_F_ext_hat_K(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
% plot(t, fr_F_dist_hat_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
plot(t, -fr_F_ext_hat_filtered(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
xlim([t_start, t_end]);
if i == 1 || i == 2 || i == 3
ylim([-20 20]);
end
if i == 4 || i == 5 || i == 6
ylim([-4 4]);
end
if i == 1 || i == 4 % 첫 번째 및 네 번째 플롯에 y축 레이블 추가
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 3
legend('DOB', 'MJCF', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
if i == 1
legend('Estimated disturb by DOB', 'Estimated disturb by MOB', 'Estimated disturb by Franka state', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
% legend('DOB', 'F_dist_hat_dob (FR3)', 'F_ext_hat_filtered', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_dist, 'Task Disturbance Estimation', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_dist, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Joint torque disturbance estiamtion
figure();
fig_ddq = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\tau_1$', '$\tau_2$', '$\tau_3$', '$\tau_4$', '$\tau_5$', '$\tau_6$', '$\tau_7$',};
plt_indices = 1:7;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, tau_dist_hat_mob(:,plt_indices(i)), 'LineStyle', '--', 'LineWidth', 1.5 * lw, 'Color', 'k');
plot(t, tau_dist_hat_dob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
plot(t, mj_tau_dist_hat(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
% plot(t, tau_c(:,plt_indices(i)) + tau_g(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
% plot(t, tau_c(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
xlim([t_start, t_end]);
% ylim([-5 5]);
ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
if i == 1
legend('MOB estimation', 'DOB estimation', 'MuJoCo', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_ddq, 'Joint Acceleration $[rad/s^2]$', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_ddq, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Comparison w/ FT sensor data
figure();
fig_dist = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\hat{F}_{ext} \ [N]$', '$\hat{T}_{ext} \ [N/m]$'};
plt_indices = 1:6;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t, fr_F_dist_hat_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
plot(t, -fr_F_ext_hat_filtered(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', 2 * lw, 'Color', 'r');
% plot(t, -fr_0_F_ext_hat_K(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', lw, 'Color', 'g');
plot(timestamps, effort_matrix(:, plt_indices(i)) - effort_offset(i), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
xlim([t_start, t_end]);
if i <= 3
ylim([-25 25]);
else
ylim([-4 4]);
end
t_patch = 15;
x_patch = [t_patch, max(t), max(t), t_patch];
y_limits = ylim; % 현재 y축 범위 가져오기
y_patch = [y_limits(1), y_limits(1), y_limits(2), y_limits(2)];
fill(x_patch, y_patch, 'cyan', 'FaceAlpha', 0.1, 'EdgeColor', 'none');
if i == 1 || i == 4
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 1
legend('DOB Estimation', 'Franka Estimation', 'FT Sensor', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_dist, 'Task Disturbance Estimation', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_dist, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
assets/data/matlab/plot_dob_data.m 0 → 100644
View file @ ec1af206
%% load data
% clc; close all; clear;
filename_dob = '../data_sim_free_dob.csv';
data_table_dob = readtable(filename_dob);
data_array_dob = table2array(data_table_dob);
vec_size_dob = [1, 7, 7, 7, 3, 3, 7, 6, 6, 6, 7, 7, 7, 7, 3, 3, 3, 3, 6, 6, 6, 6, 6, 6, 7, 7, 7, 7, 6, 6];
start_idx_dob = [1, ];
for i = 1:length(vec_size_dob) - 1
start_idx_dob(i+1) = start_idx_dob(i) + vec_size_dob(i);
end
data_cell = cell(1, length(vec_size_dob));
for i = 1:length(vec_size_dob)
data_cell{i} = data_array_dob(:, start_idx_dob(i):start_idx_dob(i) + vec_size_dob(i) - 1);
end
t_dob = data_cell{1};
ee_pos_mes_dob = data_cell{16};
ee_rpy_mes_dob = data_cell{18};
ee_vel_mes_dob = data_cell{20};
ddq_hat_mob_filtered_dob = data_cell{28};
%% Joint acceleration
figure();
fig_ddq = tiledlayout(7, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\ddot{q}_1$', '$\ddot{q}_2$', '$\ddot{q}_3$', '$\ddot{q}_4$', '$\ddot{q}_5$', ...
'$\ddot{q}_6$', '$\ddot{q}_7$',};
plt_indices = 1:7;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t_dob, ddq_eul(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 1.5 * lw, 'Color', 'k');
% plt1 = plot(t, ddq_hat_sosml(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', 2 * lw, 'Color', 'b');
% plt1.Color(4) = 0.7;
% plt2 = plot(t, ddq_mj(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
% plt2.Color(4) = 0.7;
% plot(t, ddq_hat_mob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
plot(t_dob, ddq_hat_mob_filtered_dob(:,plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
xlim([t_start, t_end]);
% ylim([-1 1]);
ylabel(ytitles(plt_indices(i)), 'Interpreter', 'latex', 'FontSize', fs_lbl)
% if i == 7
% ylim([-1 1]);
% end
if i == 1
legend('Time-derivative + LPF of $\dot{q}$', 'Estimation using Momentum Observer + LPF', ...
'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_ddq, 'Joint Acceleration $[rad/s^2]$', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_ddq, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Pose Tracking Performance
figure();
fig_pose = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$p_{ee} \ [m]$', '$\psi_{ee} \ [rad]$'};
plt_indices = 1:3;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t_dob, ee_pos_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t_dob, ee_pos_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
% plot(t, fr_ee_pos_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
xlim([t_start, t_end]);
if i == 1
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 3
legend('Desired', 'Actual', 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t_dob, ee_rpy_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t_dob, ee_rpy_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'r');
% plot(t, fr_ee_rpy_mes_dob(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'b');
xlim([t_start, t_end]);
if i == 1
ylabel(ytitles{mod(i-1, 2) + 1 + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
end
title(fig_pose, 'Pose Tracking', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_pose, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
%% Twist Tracking Performance
figure();
fig_twist = tiledlayout(6, 1, "Padding", "compact", "TileSpacing", "compact");
ytitles = {'$\dot{p}_{ee} \ [m/s]$', '$\omega_{ee} \ [rad/s]$'};
plt_indices = 1:6;
for i = 1:length(plt_indices)
nexttile;
hold on;
grid;
plot(t_dob, ee_vel_des(:, plt_indices(i)), 'LineStyle', '--', 'LineWidth', 2 * lw, 'Color', 'k');
plot(t_dob, ee_vel_mes_dob(:, plt_indices(i)), 'LineStyle', '-.', 'LineWidth', 1.5 * lw, 'Color', 'r');
% plot(t, ee_vel_mj(:, plt_indices(i)), 'LineStyle', '-', 'LineWidth', lw, 'Color', 'g');
xlim([t_start, t_end]);
if i == 1 || i == 4
ylabel(ytitles{mod(i-1, 2) + 1}, 'Interpreter', 'latex', 'FontSize', fs_lbl);
end
if i == 3
legend('Desired', 'Actual', 'MuJoCo Data' , 'FontName', font, 'FontSize', fs_lgd, 'interpreter', 'latex');
end
end
title(fig_twist, 'Twist Tracking', 'FontSize', fs_title, 'FontName', font, 'Interpreter', 'latex');
xlabel(fig_twist, 'Time (sec)', 'Interpreter', 'latex', 'FontSize', fs_lbl, 'FontName', font);
assets/images/250123-DOB-NaN.png 0 → 100644
View file @ ec1af206
assets/images/250123-DOB-NaN.png

35.1 KiB

assets/images/ft_contact_01_result.png 0 → 100644
View file @ ec1af206
assets/images/ft_contact_01_result.png

130 KiB

assets/images/ft_contact_02_result.png 0 → 100644
View file @ ec1af206
assets/images/ft_contact_02_result.png

143 KiB

assets/model/fr3.urdf deleted 100644 → 0
View file @ bf476964
This diff is collapsed.
assets/model/fr3_franka_hand.urdf 0 → 100644
View file @ ec1af206
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from /workspaces/src/franka_description/robots/fr3/fr3.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="fr3">
<!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
<!-- arm_id: Namespace of the robot arm. Serves to differentiate between arms in case of multiple instances. -->
<!-- joint_limits: description of the joint limits that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/joint_limits.yaml')} -->
<!-- kinematics: description of the kinematics that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/kinematics.yaml')} -->
<!-- inertials: description of the inertials that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/inertials.yaml')} -->
<!-- dynamics: description of the dynamics that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/dynamics.yaml')} -->
<link name="base">
</link>
<joint name="fr3_base_joint" type="fixed">
<parent link="base"/>
<child link="fr3_link0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="fr3_link0">
<visual name="fr3_link0_visual">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link0.dae"/>
</geometry>
</visual>
<collision name="fr3_link0_collision">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link0.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.0172 0.0004 0.0745"/>
<mass value="2.3966"/>
<inertia ixx="0.009" ixy="0.0" ixz="0.002" iyy="0.0115" iyz="0.0" izz="0.0085"/>
</inertial>
</link>
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="fr3_link1">
<visual name="fr3_link1_visual">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link1.dae"/>
</geometry>
</visual>
<collision name="fr3_link1_collision">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link1.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0000004128 -0.0181251324 -0.0386035970"/>
<mass value="2.9274653454"/>
<inertia ixx="0.023927316485107913" ixy="1.3317903455714081e-05" ixz="-0.00011404774918616684" iyy="0.0224821613275756" iyz="-0.0019950320628240115" izz="0.006350098258530016"/>
</inertial>
</link>
<joint name="fr3_joint1" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.333"/>
<parent link="fr3_link0"/>
<child link="fr3_link1"/>
<axis xyz="0 0 1"/>
<limit effort="87.0" lower="-2.7437" upper="2.7437" velocity="2.62"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.7437" soft_upper_limit="2.7437"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
</joint>
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="fr3_link2">
<visual name="fr3_link2_visual">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link2.dae"/>
</geometry>
</visual>
<collision name="fr3_link2_collision">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link2.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0031828864 -0.0743221644 0.0088146084"/>
<mass value="2.9355370338"/>
<inertia ixx="0.041938946257609425" ixy="0.00020257331521090626" ixz="0.004077784227179924" iyy="0.02514514885014724" iyz="-0.0042252158006570156" izz="0.06170214472888839"/>
</inertial>
</link>
<joint name="fr3_joint2" type="revolute">
<origin rpy="-1.570796326794897 0 0" xyz="0 0 0"/>
<parent link="fr3_link1"/>
<child link="fr3_link2"/>
<axis xyz="0 0 1"/>
<limit effort="87.0" lower="-1.7837" upper="1.7837" velocity="2.62"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7837" soft_upper_limit="1.7837"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
</joint>
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="fr3_link3">
<visual name="fr3_link3_visual">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link3.dae"/>
</geometry>
</visual>
<collision name="fr3_link3_collision">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link3.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0407015686 -0.0048200565 -0.0289730823"/>
<mass value="2.2449013699"/>
<inertia ixx="0.02410142547240885" ixy="0.002404694559042109" ixz="-0.002856269270114313" iyy="0.01974053266708178" iyz="-0.002104212683891874" izz="0.019044494482244823"/>
</inertial>
</link>
<joint name="fr3_joint3" type="revolute">
<origin rpy="1.570796326794897 0 0" xyz="0 -0.316 0"/>
<parent link="fr3_link2"/>
<child link="fr3_link3"/>
<axis xyz="0 0 1"/>
<limit effort="87.0" lower="-2.9007" upper="2.9007" velocity="2.62"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.9007" soft_upper_limit="2.9007"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
</joint>
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="fr3_link4">
<visual name="fr3_link4_visual">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link4.dae"/>
</geometry>
</visual>
<collision name="fr3_link4_collision">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link4.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.0459100965 0.0630492960 -0.0085187868"/>
<mass value="2.6155955791"/>
<inertia ixx="0.03452998321913202" ixy="0.01322552265982813" ixz="0.01015142998484113" iyy="0.028881621933049058" iyz="-0.0009762833870704552" izz="0.04125471171146641"/>
</inertial>
</link>
<joint name="fr3_joint4" type="revolute">
<origin rpy="1.570796326794897 0 0" xyz="0.0825 0 0"/>
<parent link="fr3_link3"/>
<child link="fr3_link4"/>
<axis xyz="0 0 1"/>
<limit effort="87.0" lower="-3.0421" upper="-0.1518" velocity="2.62"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0421" soft_upper_limit="-0.1518"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
</joint>
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="fr3_link5">
<visual name="fr3_link5_visual">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link5.dae"/>
</geometry>
</visual>
<collision name="fr3_link5_collision">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link5.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.0016039605 0.0292536262 -0.0972965990"/>
<mass value="2.3271207594"/>
<inertia ixx="0.051610278463662895" ixy="-0.005715173387783472" ixz="-0.0035673167625872135" iyy="0.04787729713371481" iyz="0.010673985108535986" izz="0.016423625579357254"/>
</inertial>
</link>
<joint name="fr3_joint5" type="revolute">
<origin rpy="-1.570796326794897 0 0" xyz="-0.0825 0.384 0"/>
<parent link="fr3_link4"/>
<child link="fr3_link5"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-2.8065" upper="2.8065" velocity="5.26"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8065" soft_upper_limit="2.8065"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
</joint>
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="fr3_link6">
<visual name="fr3_link6_visual">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link6.dae"/>
</geometry>
</visual>
<collision name="fr3_link6_collision">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link6.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0597131221 -0.0410294666 -0.0101692726"/>
<mass value="1.8170376524"/>
<inertia ixx="0.005412333594383447" ixy="0.006193456360285834" ixz="0.0014219289306117652" iyy="0.014058329545509979" iyz="-0.0013140753741120031" izz="0.016080817924212554"/>
</inertial>
</link>
<joint name="fr3_joint6" type="revolute">
<origin rpy="1.570796326794897 0 0" xyz="0 0 0"/>
<parent link="fr3_link5"/>
<child link="fr3_link6"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="0.5445" upper="4.5169" velocity="4.18"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="0.5445" soft_upper_limit="4.5169"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
</joint>
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="fr3_link7">
<visual name="fr3_link7_visual">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/visual/link7.dae"/>
</geometry>
</visual>
<collision name="fr3_link7_collision">
<geometry>
<mesh filename="package://franka_description/meshes/robot_arms/fr3/collision/link7.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0045225817 0.0086261921 -0.0161633251"/>
<mass value="0.6271432862"/>
<inertia ixx="0.00021092389150104718" ixy="-2.433299114461931e-05" ixz="4.564480393778983e-05" iyy="0.00017718568002411474" iyz="8.744070223226438e-05" izz="5.993190599659971e-05"/>
</inertial>
</link>
<joint name="fr3_joint7" type="revolute">
<origin rpy="1.570796326794897 0 0" xyz="0.088 0 0"/>
<parent link="fr3_link6"/>
<child link="fr3_link7"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-3.0159" upper="3.0159" velocity="5.26"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0159" soft_upper_limit="3.0159"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.2" mu_coulomb="0" mu_viscous="16"/>
</joint>
<link name="fr3_link8"/>
<joint name="fr3_joint8" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.107"/>
<parent link="fr3_link7"/>
<child link="fr3_link8"/>
</joint>
<joint name="fr3_hand_joint" type="fixed">
<parent link="fr3_link8"/>
<child link="fr3_hand"/>
<origin rpy="0 0 -0.7853981633974483" xyz="0 0 0"/>
</joint>
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="fr3_hand">
<visual name="fr3_hand_visual">
<geometry>
<mesh filename="package://franka_description/meshes/robot_ee/franka_hand_white/visual/hand.dae"/>
</geometry>
</visual>
<collision name="fr3_hand_collision">
<geometry>
<mesh filename="package://franka_description/meshes/robot_ee/franka_hand_white/collision/hand.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.01 0 0.03"/>
<mass value="0.73"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.0025" iyz="0" izz="0.0017"/>
</inertial>
</link>
<!-- Define the hand_tcp frame -->
<link name="fr3_hand_tcp"/>
<joint name="fr3_hand_tcp_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.1034"/>
<parent link="fr3_hand"/>
<child link="fr3_hand_tcp"/>
</joint>
</robot>
assets/model/meshes/visual/finger.dae deleted 100644 → 0
View file @ bf476964
This diff is collapsed.
assets/model/meshes/visual/hand.dae deleted 100644 → 0
View file @ bf476964
This diff is collapsed.
assets/model/meshes/visual/link0.dae deleted 100644 → 0
View file @ bf476964
This diff is collapsed.
assets/model/meshes/visual/link1.dae deleted 100644 → 0
View file @ bf476964
This diff is collapsed.
assets/model/meshes/visual/link2.dae deleted 100644 → 0
View file @ bf476964
This diff is collapsed.
assets/model/meshes/visual/link3.dae deleted 100644 → 0
View file @ bf476964
This diff is collapsed.
assets/model/meshes/visual/link4.dae deleted 100644 → 0
View file @ bf476964
This diff is collapsed.
assets/model/meshes/visual/link5.dae deleted 100644 → 0
View file @ bf476964
This diff is collapsed.

Datenschutz und Impressum