--- /dev/null
+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)
+
+
--- /dev/null
+#include <ros/ros.h>
+
+#include <Eigen/Dense>
+
+#include <std_msgs/Float64.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <sensor_msgs/JointState.h>
+
+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> 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<std_msgs::Float64MultiArray>("ident/dynamic_parameters",1000);
+ leftWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/left_wheel_command",1000);
+ rightWheelCommandPublisher=node_.advertise<std_msgs::Float64>("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;
+}
projects / twil.git / commitdiff