cnr-stiima-iras / rosdyn

The goal of the project ROSdyn is to realize a ROS-based package that implement a fully automated procedure able to calibrate the robot dynamics model. The use of ROS ecosystem enable the standardization of several steps of the procedure, such as the messages exchanged between nodes. A further step towards standardization is represented by the use of ROS-I robot drivers that allow a standard access to all the robot information enabling the dynamics model calibration on commercial IR.

ROSdyn implements a fully automated procedure able to calibrate the robot dynamics model.

It is integrated with MoveIt! to automatically compute, simulate, and execute identification trajectory. The result is stored in a URDF file.

Build/Installation

The software can be installed with the following rosinstall file.

Travis CI Kinetic Build:

IMPORTANT rosdyn_identification has been moved here.

List of packages

rosdyn_core see README:

Dynamics header library based on Eigen. With respect to KDL, it has two advantages: it is faster and it allow computing model regressor.

An example of usage can be found here

The following list shows the computation times for a 6DOF robot on a laptop Asus PU551J with Ubuntu 16.04 (Release build, average on 10000 trials).

** computation time in microseconds: **

pose = 0.75970 [us]

pose + jacobian = 1.06562 [us]

pose + jacobian + velocity twists for all links = 1.25589 [us]

pose + jacobian + velocity twists for all links + linear aceleration twists for all links = 1.25351 [us]

pose + jacobian + velocity twists for all links + non linear acceleration twists for all links = 1.51663 [us]

pose + jacobian + velocity twists for all links + acceleration twists for all links = 1.83826 [us]

pose + jacobian + velocity twists for all links + acceleration twists for all links + jerk twists for all links = 2.68916 [us]

pose + jacobian + velocity twists for all links + acceleration twists for all links + joint torque = 3.76733 [us]

pose + jacobian + joint inertia matrix = 10.06761 [us]

Acknowledgements

RosDyn is developed by CNR-STIIMA (www.stiima.cnr.it)

---

Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components.
More information: rosin-project.eu

This project has received funding from the European Union’s Horizon 2020
research and innovation programme under grant agreement no. 732287.

rosdyn's Issues

can you provide the reference papers of the project

can you provide the reference papers of the project

how can we plot the predicated torque and real torque ,and what about the results obtained ,good or bad ,and how can we get the symbolic represenation of the base param set

[ INFO] [1653647485.479994772, 175.238000000]: result
status: 0
identification:
joint_names[]
joint_names[0]: joint1
joint_names[1]: joint2
joint_names[2]: joint3
joint_names[3]: joint4
joint_names[4]: joint5
joint_names[5]: joint6
joint_names[6]: joint7
rms_error[]
rms_error[0]: 0.00130501
rms_error[1]: 0.000747481
rms_error[2]: 0.000651755
rms_error[3]: 0.000730275
rms_error[4]: 0.000160406
rms_error[5]: 0.000266742
rms_error[6]: 0.000122343
real_model_correlation[]
real_model_correlation[0]: 0.896601
real_model_correlation[1]: 0.999997
real_model_correlation[2]: 0.999948
real_model_correlation[3]: 0.999995
real_model_correlation[4]: 0.999976
real_model_correlation[5]: 0.999821
real_model_correlation[6]: 0.999622
validation:
joint_names[]
joint_names[0]: joint1
joint_names[1]: joint2
joint_names[2]: joint3
joint_names[3]: joint4
joint_names[4]: joint5
joint_names[5]: joint6
joint_names[6]: joint7
rms_error[]
rms_error[0]: 0.00099746
rms_error[1]: 0.000780719
rms_error[2]: 0.000680628
rms_error[3]: 0.000691188
rms_error[4]: 0.000147779
rms_error[5]: 0.000258036
rms_error[6]: 0.000121975
real_model_correlation[]
real_model_correlation[0]: 0.947562
real_model_correlation[1]: 0.999997
real_model_correlation[2]: 0.999942
real_model_correlation[3]: 0.999996
real_model_correlation[4]: 0.999981
real_model_correlation[5]: 0.999831
real_model_correlation[6]: 0.999634

how can we plot the predicated torque and real torque ,and what about the results obtained ,good or bad ,and how can we get the symbolic represenation of the base param set
thanks

No valid file to be loaded /root/.ros"

I have a problem. When the button 4)estimate_model pressed, the terminal shows "No valid file to be loaded /root/.ros"(seen on the left of the following picture). What kind of the file should be loaded here? I checked the source, there were two "error info"(seen on the right of the following picture). I doubt that is there something I set incorrectly? How can I define the address where I can save the estimation results?

Moreover, on the right of the above picture, in the line 148 in rosdyn_par_estim_interface.cpp, there indicates that "Find all the files that need to be loaded":

  // Find all the files that need to be loaded
  std::string dir = test_path_ + "/";
  std::vector<std::string> files = std::vector<std::string>();

  if (getdir(dir, files))
  {
    ROS_ERROR("No valid file to be loaded %s\n", test_path_.c_str());
    m_result_.status = rosdyn_identification_msgs::MetoParEstimResult::GENERIC_ERROR;
    m_meto_par_estim_as->setAborted(m_result_);
    return;
  }

while the vector "files" is empty. What kinds of files are necessary here? And, if the file would be saved in the "/root/.ros", should I launch the rviz also in the root or with a super permission?

Build error: Missing bracket in rosdyn_core?

Commit 48e1c02 introduces:

- if (m_cart_error_in_b.norm()<toll)
+ if (weight.asDiagonal()*m_cart_error_in_b.norm()<toll)

which fails to build with:

/home/matthias/Workspaces/dynamics_ws/src/rosdyn/rosdyn_core/include/rosdyn_core/primitives.h:1371:55: error: no match for ‘operator<’ (operand types are ‘const Eigen::DiagonalWrapper<const Eigen::CwiseBinaryOp<Eigen::internal::scalar_product_op<double, double>, const Eigen::Matrix<double, 6, 1>, const Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<double>, const Eigen::Matrix<double, 6, 1> > > >’ and ‘const double’)
       if (weight.asDiagonal()*m_cart_error_in_b.norm()<toll)
           ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~

Should it say

if ((weight.asDiagonal()*m_cart_error_in_b).norm()<toll)

instead?

Generated trajectories reuse timestamps and fail to execute

The generated trajectories fail to execute with MoveIT which reports:

Trajectory message contains waypoints that are not strictly increasing in time

When taking a look at rosparam get /ident_trj/time_from_start, some timestamps appear multiple times. We think they are the ones where different trajectories are combined.

We worked around this by removing them with a short python script attached below since we did not want to dig in the code right now. To run it, execute the following lines after the generation and then continue with the simulation:

rosparam dump /tmp/traj.yaml /ident_trj
python3 remove_duplicates.py
rosparam load /tmp/traj_cleaned.yaml /ident_trj/

This is remove_duplicates.py:

#! /usr/bin/env python3

import yaml
import collections

file = open("/tmp/traj.yaml")
params = yaml.load(file)

print("Available keys: " + str((params.keys())))

# We find the time stamps that appear twice
dupes = [item for item, count in collections.Counter(params.get("time_from_start")).items() if count > 1]

print("Duplicate values in time series: " + str(dupes))
print("Number of duplicates found: " + str(len(dupes)))

print("Number of points before cleanup: " + str(len(params["time_from_start"])))

for dupe in dupes:
    ind = params["time_from_start"].index(dupe)
    print("Removing index " + str(ind+1))
    params["positions"].pop(ind+1)
    params["accelerations"].pop(ind+1)
    params["velocities"].pop(ind+1)
    params["time_from_start"].pop(ind+1)

print("Number of points after cleanup: " + str(len(params["time_from_start"])))

with open("/tmp/traj_cleaned.yaml", "w") as f:
    yaml.dump(params, f)

Dimensions mismatch between the number of bytes in the binary file and the reconstructed matrix! (file name: ident_trj_193_rep1_JointState__joint_states.bin)

/joint_states topic is published by gazebo with frequency 1000hz ,but when I press estimate buttion , it console shows the info like that:

[ INFO] [1653631818.249324260, 347.150000000]: File: ident_trj_73_rep1_JointState__joint_states.bin loaded!
[ INFO] [1653631818.251855619, 347.152000000]: Loading a new binary file, please wait a moment...
[ERROR] [1653631818.260454913, 347.161000000]: Dimensions mismatch between the number of bytes in the binary file and the reconstructed matrix! (file name: ident_trj_193_rep1_JointState__joint_states.bin)
[ERROR] [1653631818.260488498, 347.161000000]: length: 25559896
[ERROR] [1653631818.260505704, 347.161000000]: rows(): 22
[ERROR] [1653631818.260515551, 347.161000000]: cols(): 145226
[ERROR] [1653631818.260525957, 347.161000000]: sizeof(double): 8

some info about /joint_states topic,

rostopic echo /joint_sates
rostopic echo /joint_states
header:
seq: 40523
stamp:
secs: 41
nsecs: 64000000
frame_id: ''
name:

• joint1
• joint2
• joint3
• joint4
• joint5
• joint6
• joint7
  position: [1.6035399585945243e-07, 4.519715526285495e-07, -7.400904440402201e-07, 3.7515162008006087e-06, 1.7652720583072323e-06, -3.6107049012912285e-06, -4.039876831285483e-06]
  velocity: [2.7011241211415602e-08, 7.006023023347753e-08, -1.2870547545497058e-07, 5.089948637724624e-07, 3.944174707883473e-07, 5.036407487998847e-07, -5.755745672579258e-07]
  effort: [-2.4169728032522206e-05, 0.0009671177411542215, -2.195733933956575e-05, -0.0005541036808418188, -3.090897781375719e-05, 6.30595178269617e-05, -8.967412755649488e-06]

---

header:
seq: 40524
stamp:
secs: 41
nsecs: 65000000
frame_id: ''
name:

• joint1
• joint2
• joint3
• joint4
• joint5
• joint6
• joint7
  position: [1.603271373440407e-07, 4.519015179837993e-07, -7.399576515965123e-07, 3.751516065797489e-06, 1.76502124737965e-06, -3.6107042662436584e-06, -4.040180536790672e-06]
  velocity: [-2.708746461885053e-08, -7.046937675559802e-08, 1.387801165462798e-07, -5.16324863542844e-07, -3.3373828886639766e-07, 4.580933332587107e-07, -3.03764013672612e-07]
  effort: [-2.3683024538698134e-05, 0.0009471518064606474, -2.1470264344563985e-05, -0.0005428775991208985, -3.0269414063676434e-05, 6.175913700311056e-05, -8.784597457168927e-06]

---

Missing dependency "configuration_msgs"

We can not build the code with melodic because the dependency for configuration_msgs can not be resolved.

The rosinstall file installs moveit_planning_helper and that one lists configuration_msgs as one of it's dependencies [1].
But neither me nor rosdep can resolve that. Could it be that this package has not been published anywhere?

As a quick workaround, I recreated the few messages that were used. It can be found there: https://git.cs.lth.se/robotlab/dynamics_identification/configuration_msgs

[1] https://github.com/CNR-STIIMA-IRAS/moveit_planning_helper/blob/6773a03da1ea2bf30b834692a08be38b0785537d/CMakeLists.txt

Estimation results window crashes RViz

RViz crashes after pressing 4) Estimate Model. The popup windows appears for a short time before RViz dies. I get these messages:

[MetoParEstimInterfaceNodelet::metoParEstimCB]: Loading a new binary file, please wait a moment...
[MetoParEstimInterfaceNodelet::metoParEstimCB]: File: ident_trj_436_rep1_JointState__bh_joint_states.bin  loaded!
[MetoParEstimInterfaceNodelet::metoParEstimCB]: Loading a new binary file, please wait a moment...
[MetoParEstimInterfaceNodelet::metoParEstimCB]: File: ident_trj_450_rep1_JointState__bh_joint_states.bin  loaded!
[MetoParEstimInterfaceNodelet::metoParEstimCB]: Starting to compute dynamics parameters...
[MetoParEstimInterfaceNodelet::metoParEstimCB]: Loaded new data file with 22 rows and 98 columns
[MetoParEstimInterfaceNodelet::metoParEstimCB]: Writing 98 rows and 7 columns, from row = 0 and col = 0
[MetoParEstimInterfaceNodelet::metoParEstimCB]: Loaded new data file with 22 rows and 98 columns
[MetoParEstimInterfaceNodelet::metoParEstimCB]: Writing 98 rows and 7 columns, from row = 98 and col = 0
[MetoParEstimInterfaceNodelet::metoParEstimCB]: Estimation complete!

in the ROSdyn terminal. Which looks normal in my opinion. And in the RVIz terminal:

rviz: QObject: Cannot create children for a parent that is in a different thread.
rviz: (Parent is Breeze::WidgetStateEngine(0x5559a8573920), parent's thread is QThread(0x5559a84d8b40), current thread is QThread(0x7ffa40001930)
rviz: QObject::installEventFilter(): Cannot filter events for objects in a different thread.
[... repeats more than 10 times]
rviz: QObject: Cannot create children for a parent that is in a different thread.
rviz: (Parent is QStackedWidget(0x7ffa40001ff0), parent's thread is QThread(0x7ffa40001930), current thread is QThread(0x5559a84d8b40)
rviz: rviz died from signal 11
rviz: rviz left a core dump

This is with an up to date ROS melodic . I did not use the melodic branch of ROSdyn since it seemed to be quite a bit behind master.

namespace

hi everyone,

I am trying to run rosdyn for a kuka lbr robot, however, the robot is launched into the namespace /lbr.

I can't get rosdyn to run in this scenario. Any ideas what to do?

Thanks a lot, best
Martin

Build error: results_popup files missing

9da4b94 introduced changes that require the files src/results_popup.cpp and results_popup.h to exist, but did not add them to the repository.

To split ROSDYN in different repositories to reduce the dependencies

Hi folks,
I was wondering if it would be possible to split the repository into three different repositories:
rosdyn_core: with the core math libraries
rosdyn_identification: with the GUI, the identification math and the msgs
rosdyn: a "fake" repository with just a rosinstall for the download of both the rosdyn_core and the rosdyn_identification to preserve back-compatibility

ERROR when I pressed button 4)estimate_model ,the terminal shows [ERROR] [1630504671.151705907]: No valid file to be loaded No valid file to be loaded /home/robot/.ros

ERROR when I pressed button 4)estimate_model ,the terminal shows [ERROR] [1630504671.151705907]: No valid file to be loaded No valid file to be loaded /home/robot/.ros
how can I solve the problem,