Skip to content
Snippets Groups Projects
Commit 941910ac authored by Jeong Woo Hong's avatar Jeong Woo Hong
Browse files

Merge branch '6-simulator-implementation' into 'main' 

Resolve "Simulator implementation"

Closes #6

See merge request !2
parents 68dcefc0 335ea70c
No related branches found
No related tags found
1 merge request!2Resolve "Simulator implementation"
Hide whitespace changes
Inline Side-by-side
test/CMakeLists.txt deleted 100644 → 0
+ 0
− 38
View file @ 68dcefc0
cmake_minimum_required(VERSION 3.10)
project(ImpedanceController)
# Set C++ standard
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED True)
# Find required packages
find_package(catkin REQUIRED COMPONENTS roscpp)
find_package(pinocchio REQUIRED)
find_package(casadi REQUIRED)
# Include directories
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# Add source files
# add_executable(pin_test interpolation-SE3.cpp)
add_executable(autodiff_test
# casadi-crnea.cpp
dynamics-autodiff-test.cpp
# inverse_kinematics_test.cpp
)
# Link libraries
# target_link_libraries(pin_test
# ${catkin_LIBRARIES}
# pinocchio::pinocchio
# casadi
# )
target_link_libraries(autodiff_test
${catkin_LIBRARIES}
pinocchio::pinocchio
casadi
)
test/dynamics-autodiff-test.cpp deleted 100644 → 0
+ 0
− 243
View file @ 68dcefc0
#include "pinocchio/autodiff/casadi.hpp"
#include "pinocchio/algorithm/aba.hpp" // joint acceleration, Minverse
#include "pinocchio/algorithm/crba.hpp" // joint-space inertia (upper-triangle)
#include "pinocchio/algorithm/jacobian.hpp" // Jacobian
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp" // Coriolis, gravity, joint torque
#include "pinocchio/parsers/urdf.hpp"
#include <iostream>
int main(int argc, char ** argv)
{
using namespace pinocchio;
//* typedef for CasADi Auto-Diff *//
typedef double Scalar;
typedef ::casadi::SX ADScalar;
typedef ModelTpl<Scalar> Model;
typedef Model::Data Data;
typedef ModelTpl<ADScalar> ADModel;
typedef ADModel::Data ADData;
typedef ADModel::ConfigVectorType ConfigVectorAD;
typedef ADModel::TangentVectorType TangentVectorAD;
//* load Franka Research 3 URDF file *//
const std::string urdf_filename = std::string("../model/fr3.urdf");
Model model;
pinocchio::urdf::buildModel(urdf_filename, model);
// std::cout << "model name: " << model.name << std::endl;
Data data(model);
// set end-effector's ID (joint 8)
const std::string ee_name = "fr_joint8";
JointIndex ee_joint_id = model.getJointId(ee_name);
if (ee_joint_id == 0)
{
std::cerr << "Invalid joint ID for end-effector: " << ee_name << std::endl;
return -1;
}
else
{
std::cout << "End-effector ID: " << ee_joint_id << std::endl; // 8
}
//* Pick up random configuration, velocity and acceleration vectors. *//
Eigen::VectorXd q = randomConfiguration(model);
Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));
Eigen::VectorXd a(Eigen::VectorXd::Random(model.nv));
std::cout << "q: " << q.transpose() << std::endl;
std::cout << "v: " << v.transpose() << std::endl;
std::cout << "a: " << a.transpose() << std::endl << std::endl;
//* Create CasADi model and data from Pinocchio model *//
ADModel ad_model = model.cast<ADScalar>();
ADData ad_data(ad_model);
//* Create symbolic CasADi vectors & Matices *//
// joint configuration
::casadi::SX cs_q = ::casadi::SX::sym("q", model.nq);
ConfigVectorAD q_ad(model.nq);
q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
// joint velocity
::casadi::SX cs_v = ::casadi::SX::sym("v", model.nv);
TangentVectorAD v_ad(model.nv);
v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
// joint acceleration
::casadi::SX cs_a = ::casadi::SX::sym("a", model.nv);
TangentVectorAD a_ad(model.nv);
a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);
// Upper-triangular Mass Matrix
::casadi::SX cs_M = ::casadi::SX::zeros(model.nv, model.nv);
// ::casadi::SX cs_M_dot = ::casadi::SX::zeros(model.nv, model.nv);
::casadi::SX cs_Minv = ::casadi::SX::zeros(model.nv, model.nv);
::casadi::SX cs_Minv_dot = ::casadi::SX::zeros(model.nv, model.nv);
// end-effector Jacobian Matrix
::casadi::SX cs_J = ::casadi::SX::zeros(6, model.nv);
::casadi::SX cs_J_dot = ::casadi::SX::zeros(6, model.nv);
//* Create CasADi "symbolic" function *//
/* forward kinematics */
// "Numerical" forward kinematics
// ! joint : universe, joint1, joint2, ... joint7 => selecting joint8 would cause segmentation
// error pinocchio::forwardKinematics(model, data, q); for (JointIndex joint_id = 0; joint_id <
// (JointIndex) model.njoints; ++joint_id)
// {
// std::cout << std::setw(24) << std::left
// << model.names[joint_id] << ": "
// << std::fixed << std::setprecision(2)
// << data.oMi[joint_id].translation().transpose()
// << std::endl;
// }
// "Symbolic" forward kinematics
// pinocchio::forwardKinematics(ad_model, ad_data, q_ad);
// for (JointIndex joint_id = 0; joint_id < (JointIndex) ad_model.njoints; ++joint_id)
// {
// std::cout << std::setw(24) << std::left
// << ad_model.names[joint_id] << ": "
// << std::fixed << std::setprecision(2)
// << ad_data.oMi[joint_id].translation().transpose()
// << std::endl;
// }
/* CRBA algorithm : compute Mass Matrix */
pinocchio::crba(ad_model, ad_data, q_ad);
for (int i = 0; i < model.nv; ++i)
{
for (int j = 0; j < model.nv; ++j)
{
cs_M(i, j) = ad_data.M(i, j); // copy symbolic M(q) into CasADi matrix
}
}
// std::cout << "Symbolic mass matrix:\n" << cs_M << std::endl;
// derivative of M w.r.t time
// for (int i = 0; i < model.nv; ++i)
// {
// for (int j = 0; j < model.nv; ++j)
// {
// ::casadi::SX dMij_dq = jacobian(cs_M(i,j), cs_q); // extract derivative of M[i, j] w.r.t
// all q cs_M_dot(i, j) = mtimes(dMij_dq, cs_v); // multiply by v and sum over nq
// }
// }
/* compute derivative of Inverse Mass Matrix */
pinocchio::computeMinverse(ad_model, ad_data, q_ad);
for (int i = 0; i < model.nv; ++i)
{
for (int j = 0; j < model.nv; ++j)
{
cs_Minv(i, j) = ad_data.Minv(i, j); // copy symbolic M(q) into CasADi matrix
}
}
for (int i = 0; i < model.nv; ++i)
{
for (int j = 0; j < model.nv; ++j)
{
::casadi::SX dMinv_dq = jacobian(cs_Minv(i, j), cs_q);
cs_Minv_dot(i, j) = mtimes(dMinv_dq, cs_v);
}
}
/* compute derivative of (end-effector) Jacobian matrix */
// !!!!! Segmentation fault
// Eigen::Matrix<ADScalar, 6, Eigen::Dynamic> J(6, model.nv);
// Data::Matrix6x J(6, model.nv);
// J.setZero();
// pinocchio::computeJointJacobian(model, data, q, ee_joint_id, J);
// pinocchio::computeJointJacobian(ad_model, ad_data, q_ad, ee_joint_id, J);
// pinocchio::computeJointJacobian(ad_model, ad_data, q_ad, ee_joint_id, ad_data.J);
// std::cout << "Jacobian J:\n" << J << std::endl;
// compute "Numerical" Jacobian
// pinocchio::computeJointJacobians(model, data, q);
// std::cout << "Jacobian J:\n" << data.J << std::endl;
// compute "Symbolic" Jacobian
pinocchio::computeJointJacobians(ad_model, ad_data, q_ad);
for (int i = 0; i < 6; ++i)
{
for (int j = 0; j < model.nv; ++j)
{
cs_J(i, j) = ad_data.J(i, j); // copy symbolic J(q) into CasADi matrix
}
}
// std::cout << "Symbolic mass matrix:\n" << cs_J << std::endl;
// Jacobian derivative
for (int i = 0; i < 6; ++i)
{
for (int j = 0; j < model.nv; ++j)
{
::casadi::SX dJij_dq =
jacobian(cs_J(i, j), cs_q); // extract derivative of J[i, j] w.r.t all q
cs_J_dot(i, j) = mtimes(dJij_dq, cs_v); // multiply by v and sum over nq
}
}
/* Inverse Dynamics -> joint torque */
pinocchio::rnea(ad_model, ad_data, q_ad, v_ad, a_ad);
::casadi::SX tau_ad(model.nv, 1);
for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
{
tau_ad(k) = ad_data.tau[k];
}
//* Build (symbolic) CasADi function for evaluation *//
::casadi::Function eval_rnea("eval_rnea", ::casadi::SXVector{ cs_q, cs_v, cs_a },
::casadi::SXVector{ tau_ad });
::casadi::Function eval_Minv_dot("eval_Minv_dot", ::casadi::SXVector{ cs_q, cs_v },
::casadi::SXVector{ cs_Minv_dot });
// std::cout << cs_Minv_dot << std::endl;
::casadi::Function eval_Jdot("eval_Jdot", ::casadi::SXVector{ cs_q, cs_v },
::casadi::SXVector{ cs_J_dot });
// std::cout << cs_J_dot << std::endl;
//* Evaluate CasADi expression with real value *//
std::vector<double> q_vec((size_t)model.nq);
Eigen::Map<Eigen::VectorXd>(q_vec.data(), model.nq, 1) = q;
std::vector<double> v_vec((size_t)model.nv);
Eigen::Map<Eigen::VectorXd>(v_vec.data(), model.nv, 1) = v;
std::vector<double> a_vec((size_t)model.nv);
Eigen::Map<Eigen::VectorXd>(a_vec.data(), model.nv, 1) = a;
// Inverse dynamics
::casadi::DM tau_casadi_res = eval_rnea(::casadi::DMVector{ q_vec, v_vec, a_vec })[0];
Data::TangentVectorType tau_casadi_vec = Eigen::Map<Data::TangentVectorType>(
static_cast<std::vector<double>>(tau_casadi_res).data(), model.nv, 1);
pinocchio::rnea(model, data, q, v, a);
std::cout << "pinocchio double:\n"
<< "\ttau = " << data.tau.transpose() << std::endl;
std::cout << "pinocchio CasADi:\n"
<< "\ttau = " << tau_casadi_vec.transpose() << std::endl;
// Minv_dot
::casadi::DM Minv_dot_res = eval_Minv_dot(::casadi::DMVector{ q_vec, v_vec })[0];
std::cout << "(CasADi) Minv_dot:" << Minv_dot_res << std::endl << std::endl;
// J_dot
::casadi::DM Jdot_res = eval_Jdot(::casadi::DMVector{ q_vec, v_vec })[0];
std::cout << "(CasADi) Jdot:" << Jdot_res << std::endl << std::endl;
pinocchio::computeJointJacobiansTimeVariation(model, data, q, v);
std::cout << "(pinocchio) Jdot:\n" << data.dJ << std::endl << std::endl;
}
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment

Datenschutz und Impressum