From: Walter Fetter Lages Date: Tue, 20 Mar 2018 17:57:17 +0000 (-0300) Subject: twil_ident: first version X-Git-Tag: 2.0.0~1 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=1a3b1aca7358f30f00eb5d4455f1fda68862181c;p=twil.git twil_ident: first version --- diff --git a/twil_ident/CMakeLists.txt b/twil_ident/CMakeLists.txt new file mode 100644 index 0000000..c50e0b4 --- /dev/null +++ b/twil_ident/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) + +find_package(Eigen REQUIRED) +include_directories(${Eigen_INCLUDE_DIRS}) +#include_directories(${EIGEN_INCLUDE_DIRS}) + +rosbuild_add_executable(ident src/ident.cpp) + + diff --git a/twil_ident/Makefile b/twil_ident/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/twil_ident/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/twil_ident/launch/ident.launch b/twil_ident/launch/ident.launch new file mode 100644 index 0000000..002625c --- /dev/null +++ b/twil_ident/launch/ident.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + diff --git a/twil_ident/mainpage.dox b/twil_ident/mainpage.dox new file mode 100644 index 0000000..bdc871e --- /dev/null +++ b/twil_ident/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b twil_ident + + + +--> + + +*/ diff --git a/twil_ident/manifest.xml b/twil_ident/manifest.xml new file mode 100644 index 0000000..109854c --- /dev/null +++ b/twil_ident/manifest.xml @@ -0,0 +1,18 @@ + + + + twil_ident + + + Walter Fetter Lages + GPL + + http://ros.org/wiki/twil_ident + + + + + + + + diff --git a/twil_ident/src/ident.cpp b/twil_ident/src/ident.cpp new file mode 100644 index 0000000..028497e --- /dev/null +++ b/twil_ident/src/ident.cpp @@ -0,0 +1,193 @@ +#include + +#include + +#include +#include +#include + +class Prbs +{ + int index; + unsigned int nd; + unsigned char *sh; + + public: + + Prbs(unsigned int n=10,unsigned int seed=0); + ~Prbs(void); + + void seed(unsigned int s) { index=s % nd; }; + + operator int(void); +}; + + +Prbs::Prbs(unsigned int n,unsigned int seed) +{ + nd=n; + index=seed % nd; + sh=new unsigned char[nd]; + for(unsigned int i=0; i < nd;i++) sh[i]=1; + for(unsigned int i=0; i < 98;i++) (int) *this; // call operator int() to exercise + +} + +Prbs::~Prbs(void) +{ +// delete[] sh; +} + +Prbs::operator int(void) +{ + unsigned char s=sh[nd-1]+sh[nd-2]; + if(s > 1) s=0; + for(int j=nd-2;j >= 0;j--) sh[j+1]=sh[j]; + sh[0]=s; + return sh[index]; +} + + +class Ident +{ + public: + Ident(ros::NodeHandle node); + ~Ident(void); + void setCommand(void); + + private: + ros::NodeHandle node_; + + ros::Subscriber jointStatesSubscriber; + ros::Publisher dynParamPublisher; + ros::Publisher leftWheelCommandPublisher; + ros::Publisher rightWheelCommandPublisher; + + const int nJoints; + + Eigen::VectorXd u; + Eigen::VectorXd thetaEst1; + Eigen::VectorXd thetaEst2; + Eigen::MatrixXd P1; + Eigen::MatrixXd P2; + + std::vector prbs; + int iter; + + ros::Time lastTime; + + void jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates); + void resetCovariance(void); +}; + + +Ident::Ident(ros::NodeHandle node): + nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),prbs(nJoints) +{ + node_=node; + + jointStatesSubscriber=node_.subscribe("/joint_states",1000,&Ident::jointStatesCB,this); + dynParamPublisher=node_.advertise("ident/dynamic_parameters",1000); + leftWheelCommandPublisher=node_.advertise("ident/left_wheel_command",1000); + rightWheelCommandPublisher=node_.advertise("ident/right_wheel_command",1000); + + u.setZero(); + thetaEst1.setZero(); + thetaEst2.setZero(); + resetCovariance(); + + lastTime=ros::Time::now(); +} + +Ident::~Ident(void) +{ + jointStatesSubscriber.shutdown(); +} + +void Ident::resetCovariance(void) +{ + P1.setIdentity(); + P1*=1; + P2.setIdentity(); + P2*=1; + iter=0; +} + +void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) +{ + ros::Duration dt=jointStates->header.stamp-lastTime; + lastTime=jointStates->header.stamp; + + Eigen::VectorXd y=-u; //y(k+1)=(u(k+1)-u(k))/dt + + Eigen::VectorXd Phi1(nJoints); + Eigen::VectorXd Phi2(nJoints); + Phi1[0]=u[1]*u[1]; // u2^2(k) + Phi2[0]=u[0]*u[1]; // u1(k)*u2(k) + + Eigen::VectorXd torque(nJoints); + for(int i=0;i < nJoints;i++) + { + u[i]=jointStates->velocity[i]; // u(k+1) + torque[i]=jointStates->effort[i]; // torque(k) + } + + y+=u; + y/=dt.toSec(); + + Phi1[1]=torque[0]+torque[1]; + Phi2[1]=torque[0]-torque[1]; + + double yEst1=Phi1.transpose()*thetaEst1; + Eigen::VectorXd K1=P1*Phi1/(1+Phi1.transpose()*P1*Phi1); + thetaEst1+=K1*(y[0]-yEst1); + P1-=K1*Phi1.transpose()*P1; + + double yEst2=Phi2.transpose()*thetaEst2; + Eigen::VectorXd K2=P2*Phi2/(1+Phi2.transpose()*P2*Phi2); + thetaEst2+=K2*(y[1]-yEst2); + P2-=K2*Phi2.transpose()*P2; + + std_msgs::Float64MultiArray dynParam; + for(int i=0;i < nJoints;i++) + { + dynParam.data.push_back(thetaEst1[i]); + dynParam.data.push_back(thetaEst2[i]); + } + for(int i=0;i < nJoints;i++) + { + dynParam.data.push_back(P1(i,i)); + dynParam.data.push_back(P2(i,i)); + } + dynParamPublisher.publish(dynParam); + +// if(iter++ > 2048) resetCovariance(); +} + +void Ident::setCommand(void) +{ + std_msgs::Float64 leftCommand; + std_msgs::Float64 rightCommand; + leftCommand.data=10.0*prbs[0]-5.0; + rightCommand.data=10.0*prbs[1]-5.0; + leftWheelCommandPublisher.publish(leftCommand); + rightWheelCommandPublisher.publish(rightCommand); +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"twil_ident"); + ros::NodeHandle node; + + Ident ident(node); + + ros::Rate loop(100); + while(ros::ok()) + { + ident.setCommand(); + + ros::spinOnce(); + loop.sleep(); + } + return 0; +}