franka를 내가 직접 움직인 후, 그것의 path를 recording해보려고 한다.
robot_state
franka로부터 얻을 수 있는 states는 굉장히 많다.
Example output:
{
"O_T_EE": [0.998578,0.0328747,-0.0417381,0,0.0335224,-0.999317,0.0149157,0,-0.04122,-0.016294,
-0.999017,0,0.305468,-0.00814133,0.483198,1],
"O_T_EE_d": [0.998582,0.0329548,-0.041575,0,0.0336027,-0.999313,0.0149824,0,-0.0410535,
-0.0163585,-0.999023,0,0.305444,-0.00810967,0.483251,1],
"F_T_EE": [0.7071,-0.7071,0,0,0.7071,0.7071,0,0,0,0,1,0,0,0,0.1034,1],
"EE_T_K": [1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1],
"m_ee": 0.73, "F_x_Cee": [-0.01,0,0.03], "I_ee": [0.001,0,0,0,0.0025,0,0,0,0.0017],
"m_load": 0, "F_x_Cload": [0,0,0], "I_load": [0,0,0,0,0,0,0,0,0],
"m_total": 0.73, "F_x_Ctotal": [-0.01,0,0.03], "I_total": [0.001,0,0,0,0.0025,0,0,0,0.0017],
"elbow": [-0.0207622,-1], "elbow_d": [-0.0206678,-1],
"tau_J": [-0.00359774,-5.08582,0.105732,21.8135,0.63253,2.18121,-0.0481953],
"tau_J_d": [0,0,0,0,0,0,0],
"dtau_J": [-54.0161,-18.9808,-64.6899,-64.2609,14.1561,28.5654,-11.1858],
"q": [0.0167305,-0.762614,-0.0207622,-2.34352,-0.0305686,1.53975,0.753872],
"dq": [0.00785939,0.00189343,0.00932415,0.0135431,-0.00220327,-0.00492024,0.00213604],
"q_d": [0.0167347,-0.762775,-0.0206678,-2.34352,-0.0305677,1.53975,0.753862],
"dq_d": [0,0,0,0,0,0,0],
"joint_contact": [0,0,0,0,0,0,0], "cartesian_contact": [0,0,0,0,0,0],
"joint_collision": [0,0,0,0,0,0,0], "cartesian_collision": [0,0,0,0,0,0],
"tau_ext_hat_filtered": [0.00187271,-0.700316,0.386035,0.0914781,-0.117258,-0.00667777,
-0.0252562],
"O_F_ext_hat_K": [-2.06065,0.45889,-0.150951,-0.482791,-1.39347,0.109695],
"K_F_ext_hat_K": [-2.03638,-0.529916,0.228266,-0.275938,0.434583,0.0317351],
"theta": [0.01673,-0.763341,-0.0207471,-2.34041,-0.0304783,1.54006,0.753865],
"dtheta": [0,0,0,0,0,0,0],
"current_errors": [], "last_motion_errors": [],
"control_command_success_rate": 0, "robot_mode": "Idle", "time": 3781435
}
아래 링크에서 자세히 알아볼 수 있다.
RobotState Struct Reference
libfranka: franka::RobotState Struct Reference
Describes the robot state. More... #include std::array< double, 16 > O_T_EE {} \(^{O}T_{EE}\) Measured end effector pose in base frame. More... std::array< double, 16 > O_T_EE_d {} \({^OT_{EE}}_{d}\) Last desired end effector pose of motion genera
frankaemika.github.io
이 중 내가 쓸 확률이 높은 것은 아래 두 가지이다.
- O_T_EE: baes frame에 대한 end effector의 pose. 4x4 matrix를 column-major format으로 나타냄
- q: 측정된 joint들의 position. 단위: rad
example output있는
https://frankaemika.github.io/docs/getting_started.html
Getting started — Franka Control Interface (FCI) documentation
This section describes how to set up a static IP address on Ubuntu 16.04 using the GUI. Follow the official Ubuntu guide if you prefer to use the command line. First, go to Network Connection widget. Select the wired connection you will be using and click
frankaemika.github.io
Path recording
robot의 state를 기록하는 cpp파일을 새로 만든다.
#include <array>
#include <cmath>
#include <functional>
#include <iostream>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <fstream>
#include <functional>
#include <iostream>
#include <iterator>
#include <mutex>
#include <random>
#include <thread>
#include <valarray>
#include <Eigen/Dense>
#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>
#include "examples_common.h"
// Modified franka example to store demonstrated motion
int main(int argc, char **argv)
{
// Check whether the required arguments were passed
if (argc != 3)
{
std::cerr << "Usage: " << argv[0] << " <robot-hostname> <outfile>" << std::endl;
return -1;
}
// Compliance parameters
const double translational_stiffness{0};
const double rotational_stiffness{0.0};
Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
stiffness.setZero();
stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
damping.setZero();
damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) * Eigen::MatrixXd::Identity(3, 3);
// Initialize file
std::ofstream myfile;
myfile.open(argv[2], std::ios::out);
try
{
// connect to robot
franka::Robot robot(argv[1]);
robot.automaticErrorRecovery();
setDefaultBehavior(robot);
// load the kinematics and dynamics model
franka::Model model = robot.loadModel();
franka::RobotState initial_state = robot.readOnce();
// equilibrium point is the initial position
Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
Eigen::Vector3d position_d(initial_transform.translation());
Eigen::Quaterniond orientation_d(initial_transform.linear());
// set collision behavior
robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
{{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});
// define callback for the torque control loop
std::function<franka::Torques(const franka::RobotState &, franka::Duration)> impedance_control_callback =
[&](const franka::RobotState &robot_state, franka::Duration /*duration*/) -> franka::Torques {
// File to store the states
for (int i = 0; i < 7; i++)
{
myfile << robot_state.q[i] << " ";
}
myfile << std::endl;
// get state variables
std::array<double, 7> coriolis_array = model.coriolis(robot_state);
std::array<double, 42> jacobian_array = model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
// convert to Eigen
Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
Eigen::Vector3d position(transform.translation());
Eigen::Quaterniond orientation(transform.linear());
// compute error to desired equilibrium pose
// position error
Eigen::Matrix<double, 6, 1> error;
error.head(3) << position - position_d;
// orientation error
// "difference" quaternion
if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0)
{
orientation.coeffs() << -orientation.coeffs();
}
// "difference" quaternion
Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
// Transform to base frame
error.tail(3) << -transform.linear() * error.tail(3);
// compute control
Eigen::VectorXd tau_task(7), tau_d(7);
// Spring damper system with damping ratio=1
tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
tau_d << tau_task + coriolis;
std::array<double, 7> tau_d_array{};
Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
return tau_d_array;
};
// start real-time control loop
std::cout << "WARNING: Collision thresholds are set to high values. "
<< "Make sure you have the user stop at hand!" << std::endl
<< "After starting try to push the robot and see how it reacts." << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
robot.control(impedance_control_callback);
}
catch (const franka::Exception &ex)
{
// print exception
std::cout << ex.what() << std::endl;
myfile.close();
}
return 0;
}위 cpp파일은 myfile에 robot_state 중 joint의 position을 기록하도록 한다.
위 파일을 examples 폴더에 넣고 make 한다.
그런 다음 text파일 이름과 함께 예제를 실행시키면 text file에 값들이 저장된다.
username@computername:~/libfranka/build/examples$ cmake ..
username@computername:~/libfranka/build/examples$ make
username@computername:~/libfranka/build/examples$ ./path_recorder 192.xxx.x.xxx t_o.txt
'RobotGrasping' 카테고리의 다른 글
| [ViSP] CoppeliaSim으로 Franka 로봇 시뮬레이션 (1) | 2023.05.30 |
|---|---|
| [FRANKA EMIKA] 초기 세팅 (0) | 2023.04.27 |
| [Meshroom] 사진으로 3D mesh 생성하기 (0) | 2023.04.24 |
| [ROS] 개발환경 구축_윈도우에서 가상머신으로 Ubuntu 설치, ROS noetic 설치 (0) | 2023.04.20 |
| [Meshlab] 설치 및 그래픽카드, GPU memory 설정 (0) | 2023.04.20 |
