Skip to content
Snippets Groups Projects
Commit 897e687c authored by Pep Martí Saumell's avatar Pep Martí Saumell
Browse files

joint limit, error bigger than 0

parent 53ec9141
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 17
− 4
View file @ 897e687c
...@@ -2,9 +2,9 @@ ...@@ -2,9 +2,9 @@
CMAKE_MINIMUM_REQUIRED(VERSION 2.4) CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
if(COMMAND cmake_policy) if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW) cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW) cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy) endif(COMMAND cmake_policy)
# The project name and the type of project # The project name and the type of project
...@@ -15,12 +15,26 @@ SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) ...@@ -15,12 +15,26 @@ SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local) SET(CMAKE_INSTALL_PREFIX /usr/local)
IF (NOT CMAKE_BUILD_TYPE) IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "RELEASE") SET(CMAKE_BUILD_TYPE "RELEASE")
ENDIF (NOT CMAKE_BUILD_TYPE) ENDIF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT") SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT") SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
#Set compiler according C++11 support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
elseif(COMPILER_SUPPORTS_CXX0X)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
ADD_SUBDIRECTORY(src) ADD_SUBDIRECTORY(src)
FIND_PACKAGE(Doxygen) FIND_PACKAGE(Doxygen)
...@@ -85,4 +99,3 @@ ELSE(UNIX) ...@@ -85,4 +99,3 @@ ELSE(UNIX)
TARGET uninstall TARGET uninstall
) )
ENDIF(UNIX) ENDIF(UNIX)
src/CMakeLists.txt
+ 8
− 8
View file @ 897e687c
# driver source files # driver source files
SET(sources common_fc.cpp kinematics.cpp uam_task_ctrl.cpp) SET(sources common_fc.cpp kinematics.cpp uam_task_ctrl.cpp)
SET(sources_tasks tasks/ir.cpp tasks/vs.cpp tasks/cog.cpp tasks/jl.cpp tasks/eepos.cpp tasks/heading.cpp tasks/basepos.cpp) SET(sources_tasks tasks/ir.cpp tasks/vs.cpp tasks/cog.cpp tasks/jl.cpp tasks/eepos.cpp
tasks/heading.cpp tasks/basepos.cpp tasks/jl_ineq.cpp)
# application header files # application header files
SET(headers common_obj.h common_fc.h kinematics.h uam_task_ctrl.h) SET(headers common_obj.h common_fc.h kinematics.h uam_task_ctrl.h)
SET(headers_tasks tasks/ir.h tasks/vs.h tasks/cog.h tasks/jl.h tasks/eepos.h tasks/heading.h tasks/basepos.h) SET(headers_tasks tasks/ir.h tasks/vs.h tasks/cog.h tasks/jl.h tasks/eepos.h
tasks/heading.h tasks/basepos.h tasks/jl_ineq.h)
# locate the necessary dependencies # locate the necessary dependencies
...@@ -12,23 +14,21 @@ SET(headers_tasks tasks/ir.h tasks/vs.h tasks/cog.h tasks/jl.h tasks/eepos.h tas ...@@ -12,23 +14,21 @@ SET(headers_tasks tasks/ir.h tasks/vs.h tasks/cog.h tasks/jl.h tasks/eepos.h tas
INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake) INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
# Boost ####### # Boost #######
set(Boost_USE_STATIC_LIBS ON) set(Boost_USE_STATIC_LIBS ON)
set(Boost_USE_MULTITHREADED OFF) set(Boost_USE_MULTITHREADED OFF)
set(Boost_USE_STATIC_RUNTIME OFF) set(Boost_USE_STATIC_RUNTIME OFF)
FIND_PACKAGE(Boost REQUIRED) FIND_PACKAGE(Boost REQUIRED)
# Eigen ####### # Eigen #######
FIND_PACKAGE(Eigen3 REQUIRED) FIND_PACKAGE(Eigen3 REQUIRED)
SET(CMAKE_BUILD_TYPE release)
# add the necessary include directories # add the necessary include directories
INCLUDE_DIRECTORIES(. ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS}) INCLUDE_DIRECTORIES(. ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS})
# create the shared library # create the shared library
ADD_LIBRARY(uam_task_ctrl SHARED ${sources} ${sources_tasks}) ADD_LIBRARY(uam_task_ctrl SHARED ${sources} ${sources_tasks})
TARGET_LINK_LIBRARIES(uam_task_ctrl ${Boost_LIBRARIES} ${orocos_kdl_LIBRARIES}) TARGET_LINK_LIBRARIES(uam_task_ctrl ${Boost_LIBRARIES} ${orocos_kdl_LIBRARIES})
# link necessary libraries # link necessary libraries
INSTALL(TARGETS uam_task_ctrl INSTALL(TARGETS uam_task_ctrl
......
src/tasks/eepos.cpp
+ 55
− 22
View file @ 897e687c
...@@ -23,12 +23,12 @@ Eigen::Vector3d CTaskEEPOS::RToEulerMin(const Eigen::Matrix3d& Rot) ...@@ -23,12 +23,12 @@ Eigen::Vector3d CTaskEEPOS::RToEulerMin(const Eigen::Matrix3d& Rot)
theta1 = -asin(Rot(2,0)); theta1 = -asin(Rot(2,0));
theta2 = 3.141592653589793 - theta1; theta2 = 3.141592653589793 - theta1;
psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1)); psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1));
psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2)); psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2));
phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1)); phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1));
phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2)); phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2));
// TODO: This statement can cause big discontinuities in the obtained angles when // TODO: This statement can cause big discontinuities in the obtained angles when
// the two norms are similar. // the two norms are similar.
if (sqrt(std::pow(theta1,2)+std::pow(psi1,2)+std::pow(phi1,2)) < sqrt(std::pow(theta2,2)+std::pow(psi2,2)+std::pow(phi2,2))) if (sqrt(std::pow(theta1,2)+std::pow(psi1,2)+std::pow(phi1,2)) < sqrt(std::pow(theta2,2)+std::pow(psi2,2)+std::pow(phi2,2)))
ang << psi1, theta1, phi1; ang << psi1, theta1, phi1;
...@@ -42,12 +42,12 @@ Eigen::Vector3d CTaskEEPOS::RToEulerMin(const Eigen::Matrix3d& Rot) ...@@ -42,12 +42,12 @@ Eigen::Vector3d CTaskEEPOS::RToEulerMin(const Eigen::Matrix3d& Rot)
if (Rot(2,0)==-1.0) if (Rot(2,0)==-1.0)
{ {
theta = 3.141592653589793/2; theta = 3.141592653589793/2;
psi = phi + atan2(Rot(0,1),Rot(0,2)); psi = phi + atan2(Rot(0,1),Rot(0,2));
} }
else else
{ {
theta = -3.141592653589793/2; theta = -3.141592653589793/2;
psi = -phi + atan2(-Rot(0,1),-Rot(0,2)); psi = -phi + atan2(-Rot(0,1),-Rot(0,2));
} }
ang << phi, theta, psi; ang << phi, theta, psi;
} }
...@@ -69,10 +69,10 @@ Eigen::Vector3d CTaskEEPOS::RToEulerContinuous(const Eigen::Matrix3d& Rot, const ...@@ -69,10 +69,10 @@ Eigen::Vector3d CTaskEEPOS::RToEulerContinuous(const Eigen::Matrix3d& Rot, const
theta1 = -asin(Rot(2,0)); theta1 = -asin(Rot(2,0));
theta2 = 3.141592653589793 - theta1; theta2 = 3.141592653589793 - theta1;
psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1)); psi1 = atan2(Rot(2,1)/cos(theta1),Rot(2,2)/cos(theta1));
psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2)); psi2 = atan2(Rot(2,1)/cos(theta2),Rot(2,2)/cos(theta2));
phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1)); phi1 = atan2(Rot(1,0)/cos(theta1),Rot(0,0)/cos(theta1));
phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2)); phi2 = atan2(Rot(1,0)/cos(theta2),Rot(0,0)/cos(theta2));
ang1 << psi1, theta1, phi1; ang1 << psi1, theta1, phi1;
ang2 << psi2, theta2, phi2; ang2 << psi2, theta2, phi2;
...@@ -89,19 +89,22 @@ Eigen::Vector3d CTaskEEPOS::RToEulerContinuous(const Eigen::Matrix3d& Rot, const ...@@ -89,19 +89,22 @@ Eigen::Vector3d CTaskEEPOS::RToEulerContinuous(const Eigen::Matrix3d& Rot, const
if (Rot(2,0)==-1.0) if (Rot(2,0)==-1.0)
{ {
theta = 3.141592653589793/2; theta = 3.141592653589793/2;
psi = phi + atan2(Rot(0,1),Rot(0,2)); psi = phi + atan2(Rot(0,1),Rot(0,2));
} }
else else
{ {
theta = -3.141592653589793/2; theta = -3.141592653589793/2;
psi = -phi + atan2(-Rot(0,1),-Rot(0,2)); psi = -phi + atan2(-Rot(0,1),-Rot(0,2));
} }
ang << phi, theta, psi; ang << phi, theta, psi;
} }
return ang; return ang;
} }
void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::MatrixXd& vs_delta_gain, const double& vs_alpha_min, const Eigen::MatrixXd& eepos_des, Eigen::MatrixXd& sigmaEEPOS, Eigen::MatrixXd& JEEPOS_reduced, Eigen::MatrixXd& JEEPOS_pseudo_reduced) void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::MatrixXd& vs_delta_gain,
const double& vs_alpha_min, const Eigen::MatrixXd& eepos_des,
Eigen::MatrixXd& sigmaEEPOS, Eigen::MatrixXd& JEEPOS_reduced,
Eigen::MatrixXd& JEEPOS_pseudo_reduced)
{ {
// Initialize sizes // Initialize sizes
sigmaEEPOS = Eigen::MatrixXd::Zero(6,1); sigmaEEPOS = Eigen::MatrixXd::Zero(6,1);
...@@ -110,11 +113,41 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M ...@@ -110,11 +113,41 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
// Task Error ________________________________________________ // Task Error ________________________________________________
// End-Effector Position Error // End-Effector Position Error
sigmaEEPOS.block(0,0,3,1) = eepos_des.block(0,0,3,1) - HT.cam_in_w.block(0,3,3,1); sigmaEEPOS.block(0,0,3,1) = eepos_des.block(0,0,3,1) - HT.cam_in_w.block(0,3,3,1);
// End-Effector Orientation Error // End-Effector Orientation Error
sigmaEEPOS.block(3,0,3,1) = eepos_des.block(3,0,3,1) - RToEulerContinuous(HT.cam_in_w.block(0,0,3,3),eepos_des.block(3,0,3,1)); // sigmaEEPOS.block(3,0,3,1) = eepos_des.block(3,0,3,1) - RToEulerContinuous(HT.cam_in_w.block(0,0,3,3),eepos_des.block(3,0,3,1));
// // Position and orientation error
// Eigen::Translation<double,3> p_des(eepos_des(0,0), eepos_des(1,0), eepos_des(2,0));
// Eigen::Quaterniond q_des = Eigen::AngleAxisd(eepos_des(3,0), Eigen::Vector3d::UnitX())
// * Eigen::AngleAxisd(eepos_des(4,0), Eigen::Vector3d::UnitY())
// * Eigen::AngleAxisd(eepos_des(5,0), Eigen::Vector3d::UnitZ());
// Eigen::Transform<double, 3, Eigen::Affine> HT_des_in_w = p_des*q_des;
//
// Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_w(HT.cam_in_w);
//
// Eigen::Transform<double, 3, Eigen::Affine> HT_w_in_des = HT_des_in_w.inverse();
//
// Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_des = HT_robot_in_w*HT_w_in_des;
//
//
// Eigen::Transform<double, 3, Eigen::Affine> HT_aux = HT_des_in_w.inverse()*HT_robot_in_w;
// Eigen::Transform<double, 3, Eigen::Affine> HT_error = HT_aux.inverse();
// Eigen::Translation<double,3> p_curr_in_w(0.0,0.0,0.0);
// Eigen::Transform<double, 3, Eigen::Affine> HT_robot_in_w_in_w;
// HT_robot_in_w_in_w = HT_robot_in_w;
// HT_robot_in_w_in_w.translation() = p_curr_in_w;
//
// HT_error = HT_robot_in_w_in_w*HT_error;
// sigmaEEPOS.block(0,0,3,1) = HT_des_in_w.rotation() * HT_robot_in_des.translation();
// std::cout << "Error" << '\n' << sigmaEEPOS.block(0,0,3,1) << '\n';
// sigmaEEPOS.block(3,0,3,1) = HT_error.rotate(HT_robot_in_w.inverse().rotation()).rotation().eulerAngles(0, 1, 2);
// Task Jacobian _____________________________________________ // Task Jacobian _____________________________________________
...@@ -126,7 +159,7 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M ...@@ -126,7 +159,7 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
Eigen::Vector3d euRb_in_w = RToEulerMin(Rb_in_w); Eigen::Vector3d euRb_in_w = RToEulerMin(Rb_in_w);
Eigen::Vector3d euRe_in_w = RToEulerMin(Re_in_w); Eigen::Vector3d euRe_in_w = RToEulerMin(Re_in_w);
Eigen::Matrix3d wTb = UAM::CCommon_Fc::GetWronskian(euRb_in_w(0),euRb_in_w(1)); Eigen::Matrix3d wTb = UAM::CCommon_Fc::GetWronskian(euRb_in_w(0),euRb_in_w(1));
Eigen::Matrix3d wTb_inv = wTb.inverse(); Eigen::Matrix3d wTb_inv = wTb.inverse();
Eigen::Matrix3d wTe = UAM::CCommon_Fc::GetWronskian(euRe_in_w(0),euRe_in_w(1)); Eigen::Matrix3d wTe = UAM::CCommon_Fc::GetWronskian(euRe_in_w(0),euRe_in_w(1));
...@@ -138,9 +171,9 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M ...@@ -138,9 +171,9 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
// Aux_Frame_Matrix1(1,1) = -1.0; // Aux_Frame_Matrix1(1,1) = -1.0;
// Aux_Frame_Matrix1(2,0) = 1.0; // Aux_Frame_Matrix1(2,0) = 1.0;
//JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(Aux_Frame_Matrix1*pe_in_b))*wTb_inv; //JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(Aux_Frame_Matrix1*pe_in_b))*wTb_inv;
Eigen::MatrixXd JEEPOS_omw = Eigen::MatrixXd::Zero(6,6+arm.nj); Eigen::MatrixXd JEEPOS_omw = Eigen::MatrixXd::Zero(6,6+arm.nj);
JEEPOS_omw.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3); JEEPOS_omw.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
JEEPOS_omw.block(3,0,3,3) = Eigen::MatrixXd::Zero(3,3); JEEPOS_omw.block(3,0,3,3) = Eigen::MatrixXd::Zero(3,3);
JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(pe_in_b))*wTb_inv; JEEPOS_omw.block(0,3,3,3) = -Rb_in_w*(UAM::CCommon_Fc::Skew(pe_in_b))*wTb_inv;
JEEPOS_omw.block(3,3,3,3) = Re_in_w*Re_in_b.transpose()*wTb_inv; JEEPOS_omw.block(3,3,3,3) = Re_in_w*Re_in_b.transpose()*wTb_inv;
...@@ -163,9 +196,9 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M ...@@ -163,9 +196,9 @@ void CTaskEEPOS::TaskErrorJac(const UAM::CHT& HT, UAM::CArm& arm, const Eigen::M
Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6); Eigen::MatrixXd omega2euldiff_in_w = Eigen::MatrixXd::Zero(6,6);
omega2euldiff_in_w.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3); omega2euldiff_in_w.block(0,0,3,3) = Eigen::MatrixXd::Identity(3,3);
omega2euldiff_in_w.block(3,3,3,3) = wTe*Re_in_w.transpose(); omega2euldiff_in_w.block(3,3,3,3) = wTe*Re_in_w.transpose();
JEEPOS.block(0,0,6,6) = omega2euldiff_in_w*JEEPOS_omw.block(0,0,6,6); JEEPOS.block(0,0,6,6) = omega2euldiff_in_w*JEEPOS_omw.block(0,0,6,6);
JEEPOS.block(0,6,6,arm.nj) = omega2euldiff_in_w*JEEPOS_omw.block(0,6,6,arm.nj); JEEPOS.block(0,6,6,arm.nj) = omega2euldiff_in_w*JEEPOS_omw.block(0,6,6,arm.nj);
// task jacobian pseudo inverse // task jacobian pseudo inverse
JEEPOS_pseudo = UAM::CCommon_Fc::WeightInvJac(JEEPOS,vs_delta_gain,vs_alpha_min,sigmaEEPOS.block(0,0,3,1).norm()); JEEPOS_pseudo = UAM::CCommon_Fc::WeightInvJac(JEEPOS,vs_delta_gain,vs_alpha_min,sigmaEEPOS.block(0,0,3,1).norm());
......
src/tasks/jl.cpp
+ 7
− 6
View file @ 897e687c
...@@ -25,7 +25,8 @@ Eigen::MatrixXd CTaskJL::TaskErrorCommon(const UAM::CArm& arm, const double& pow ...@@ -25,7 +25,8 @@ Eigen::MatrixXd CTaskJL::TaskErrorCommon(const UAM::CArm& arm, const double& pow
return AAL; return AAL;
} }
void CTaskJL::TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des, Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo) void CTaskJL::TaskErrorJac(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos_des,
Eigen::MatrixXd& jntlim_pos_error, Eigen::MatrixXd& sigmaL, Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo)
{ {
// Get common elements // Get common elements
double pow_n = 4; double pow_n = 4;
...@@ -55,18 +56,18 @@ void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos ...@@ -55,18 +56,18 @@ void CTaskJL::TaskErrorJacFull(UAM::CArm& arm, const Eigen::MatrixXd& jntlim_pos
TaskErrorCommon(arm,pow_n,jntlim_pos_des,jntlim_pos_error,AAL); TaskErrorCommon(arm,pow_n,jntlim_pos_des,jntlim_pos_error,AAL);
sigmaL = Eigen::MatrixXd::Zero(arm.nj,1); sigmaL = Eigen::MatrixXd::Zero(arm.nj,1);
JL = Eigen::MatrixXd::Zero(arm.nj,4+arm.nj); JL = Eigen::MatrixXd::Zero(arm.nj,4+arm.nj);
for (unsigned int ii = 0; ii < arm.nj; ++ii) for (unsigned int ii = 0; ii < arm.nj; ++ii)
{ {
// Task sigma // Task sigma
sigmaL(ii,0) = -AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n); sigmaL(ii,0) = -AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n);
// Task Jacobian // Task Jacobian
JL(ii,4+ii) = -pow_n*AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n-1); JL(ii,4+ii) = -pow_n*AAL(ii,ii)*std::pow(jntlim_pos_error(ii,0),pow_n-1);
} }
// Task Jacobian Pseudo-inverse // Task Jacobian Pseudo-inverse
JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL); JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
} }
} // End of UAM namespace } // End of UAM namespace
\ No newline at end of file
src/tasks/jl_ineq.cpp 0 → 100644
+ 41
− 0
View file @ 897e687c
// Ownsr/include/eigen3/Eigen/src/Core/Product.h:133: Eigen::Product<Lhs, Rhs, Option>::Product(const Lhs&, const Rhs&) [with _Lhs = Eigen::Block<Eigen::Matrix<double, -1, -1>, 1, -1, false>; _Rh
#include "jl_ineq.h"
#include <common_fc.h>
namespace UAM {
CTaskJLIn::CTaskJLIn(){}
CTaskJLIn::~CTaskJLIn(){}
void CTaskJLIn::TaskErrorJacFull(UAM::CArm& arm,
const Eigen::MatrixXd& jntlim_ub, const Eigen::MatrixXd& jntlim_lb,
Eigen::MatrixXd& sigmaL,
Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo)
{
sigmaL = Eigen::MatrixXd::Zero(2*arm.nj,1);
JL = Eigen::MatrixXd::Zero(arm.nj*2,4+arm.nj);
JL.block(0, 4, arm.nj, arm.nj) = Eigen::MatrixXd::Identity(arm.nj, arm.nj);
JL.block(arm.nj, 4, arm.nj, arm.nj) = -1*Eigen::MatrixXd::Identity(arm.nj, arm.nj);
// JL.block(0, 4, arm.nj, arm.nj) = -1*Eigen::MatrixXd::Identity(arm.nj, arm.nj);
// JL.block(arm.nj, 4, arm.nj, arm.nj) = Eigen::MatrixXd::Identity(arm.nj, arm.nj);
for (unsigned int ii = 0; ii < arm.nj; ++ii)
{
// Task sigma
// sigmaL(ii,0) = arm.jnt_pos(ii,0) - jntlim_ub(ii,0); // q-ub<=0
// sigmaL(ii + arm.nj,0) = jntlim_lb(ii,0) - arm.jnt_pos(ii,0); // lb-q<=0
// Note: Although the ideal formulation should consider the above equations,
// we formulate the errors as e >= 0 to keep compatibility with the other tasks
// which are also defined as e >= 0.
// Inside the solver code there is a fix to solve for e<=0.
sigmaL(ii,0) = jntlim_ub(ii,0) - arm.jnt_pos(ii,0); // ub-q>=0
sigmaL(ii + arm.nj,0) = arm.jnt_pos(ii,0) - jntlim_lb(ii,0); // q-lb>=0
}
// Task Jacobian Pseudo-inverse
JL_pseudo = UAM::CCommon_Fc::CalcPinv(JL);
}
} // End of UAM namespace
src/tasks/jl_ineq.h 0 → 100644
+ 49
− 0
View file @ 897e687c
#ifndef _TASK_JLIN_H
#define _TASK_JLIN_H
// Eigen
#include <eigen3/Eigen/Dense>
// Own
#include <common_obj.h>
namespace UAM {
class CTaskJLIn
{
private:
public:
/**
* \brief Constructor
*
* Class constructor.
*
*/
CTaskJLIn();
/**
* \brief Destructor
*
* Class destructor.
*
*/
~CTaskJLIn();
/**
* \brief Task Error and Jacobians
*
* Returns the task error and Jacobians. Both error and Jacobians are computed in the same functions to avoid repeated operations.
* The returned Jacobian has full size (Arm DoF rows) and similarly the pseudo inverse of the jacobian.
*
*/
static void TaskErrorJacFull(UAM::CArm& arm,
const Eigen::MatrixXd& jntlim_ub, const Eigen::MatrixXd& jntlim_lb,
Eigen::MatrixXd& sigmaL,
Eigen::MatrixXd& JL, Eigen::MatrixXd& JL_pseudo);
};
} // End of UAM namespace
#endif
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