From: Walter Fetter Lages Date: Wed, 21 Mar 2018 16:07:44 +0000 (-0300) Subject: ROSBook 2016 initial submision X-Git-Tag: rosbook2016initial^0 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=096d818f5b8927675b31d5ed6998bfe0d52934af;p=twil.git ROSBook 2016 initial submision twil_description: remove doc twil_ident: line wrap remove covariance reset twil_controllers: remove cart_linearizing_controller_rne remove nosmooth_backstep_controller remove adaptive_nonsmooth_backstep --- diff --git a/twil_controllers/CMakeLists.txt b/twil_controllers/CMakeLists.txt index 7261bb6..2b39e24 100644 --- a/twil_controllers/CMakeLists.txt +++ b/twil_controllers/CMakeLists.txt @@ -101,8 +101,6 @@ include_directories(include ## Declare a cpp library add_library(twil_controllers src/cart_linearizing_controller.cpp - src/cart_linearizing_controller_rne.cpp - src/nonsmooth_backstep_controller.cpp ) ## Declare a cpp executable diff --git a/twil_controllers/config/nonsmooth_backstep_control.yaml b/twil_controllers/config/nonsmooth_backstep_control.yaml deleted file mode 100644 index d5761f9..0000000 --- a/twil_controllers/config/nonsmooth_backstep_control.yaml +++ /dev/null @@ -1,11 +0,0 @@ -twil: - - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - - nonsmooth_backstep_controller: - type: twil_controllers/NonSmoothBackstepController - joints: - - left_wheel_joint - - right_wheel_joint diff --git a/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h b/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h deleted file mode 100644 index 20df5d3..0000000 --- a/twil_controllers/include/twil_controllers/cart_linearizing_controller_rne.h +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_H -#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_H - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - - -namespace twil_controllers -{ - class CartLinearizingController_RNE: public controller_interface::Controller - { - public: - CartLinearizingController_RNE(void); - ~CartLinearizingController_RNE(void); - - bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); - void starting(const ros::Time& time); - void update(const ros::Time& time,const ros::Duration& duration); - - private: - ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; - std::vector joints_; - - void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command); - ros::Subscriber sub_command_; - - KDL::ChainIdSolver_RNE *idsolver; - - KDL::JntArray phi; - KDL::JntArray nu; - KDL::JntArray dnu; - KDL::JntArray torque; - std::vector v; - - KDL::Wrenches fext; - - std::vector wheelRadius; - double wheelBase; - }; -} -#endif diff --git a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h deleted file mode 100644 index 4b9c205..0000000 --- a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H -#define TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -namespace twil_controllers -{ - class NonSmoothBackstepController: public controller_interface::Controller - { - public: - NonSmoothBackstepController(void); - ~NonSmoothBackstepController(void); - - bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); - void starting(const ros::Time& time); - void update(const ros::Time& time,const ros::Duration& duration); - - private: - ros::NodeHandle node_; - hardware_interface::EffortJointInterface *robot_; - std::vector joints_; - - ros::Subscriber sub_command_; - ros::Subscriber sub_parameters_; - - Eigen::Matrix2d Ginv; - Eigen::Matrix2d F; - - std::vector wheelRadius; - double wheelBase; - - Eigen::Vector3d xi; - Eigen::Vector3d xiRef; - - Eigen::Vector2d eta; - - void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command); - void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command); - }; -} -#endif diff --git a/twil_controllers/launch/adaptive_nonsmooth_backstep.launch b/twil_controllers/launch/adaptive_nonsmooth_backstep.launch deleted file mode 100644 index fb8904e..0000000 --- a/twil_controllers/launch/adaptive_nonsmooth_backstep.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/twil_controllers/launch/cart_linearizing.launch b/twil_controllers/launch/cart_linearizing.launch index 65803f7..8e08ffe 100644 --- a/twil_controllers/launch/cart_linearizing.launch +++ b/twil_controllers/launch/cart_linearizing.launch @@ -11,6 +11,6 @@ - + diff --git a/twil_controllers/launch/nonsmooth_backstep.launch b/twil_controllers/launch/nonsmooth_backstep.launch deleted file mode 100644 index f8f3a80..0000000 --- a/twil_controllers/launch/nonsmooth_backstep.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/twil_controllers/src/cart_linearizing_controller_rne.cpp b/twil_controllers/src/cart_linearizing_controller_rne.cpp deleted file mode 100644 index 31fc7e7..0000000 --- a/twil_controllers/src/cart_linearizing_controller_rne.cpp +++ /dev/null @@ -1,159 +0,0 @@ -#include -#include - -#include -#include -#include - -namespace twil_controllers -{ - CartLinearizingController_RNE::CartLinearizingController_RNE(void): - phi(0),nu(0),dnu(0),torque(0),v(0),fext(0),wheelRadius(0) - { - } - - CartLinearizingController_RNE::~CartLinearizingController_RNE(void) - { - sub_command_.shutdown(); - } - - bool CartLinearizingController_RNE::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n) - { - node_=n; - robot_=robot; - - XmlRpc::XmlRpcValue joint_names; - if(!node_.getParam("joints",joint_names)) - { - ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) - { - ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - for(int i=0; i < joint_names.size();i++) - { - XmlRpc::XmlRpcValue &name_value=joint_names[i]; - if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString) - { - ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); - joints_.push_back(j); - v.push_back(0); - } - sub_command_ = node_.subscribe("command",1000,&CartLinearizingController_RNE::commandCB,this); - - std::string robot_desc_string; - if(!node_.getParam("/robot_description",robot_desc_string)) - { - ROS_ERROR("Could not find '/robot_description'."); - return false; - } - - KDL::Tree tree; - if (!kdl_parser::treeFromString(robot_desc_string,tree)) - { - ROS_ERROR("Failed to construct KDL tree."); - return false; - } - - KDL::Chain chain; - if (!tree.getChain("left_wheel","right_wheel",chain)) - { - ROS_ERROR("Failed to get chain from KDL tree."); - return false; - } - - // Get gravity from gazebo or set values if not simulating - KDL::Vector g; - node_.param("/gazebo/gravity_x",g[1],0.0); - node_.param("/gazebo/gravity_y",g[1],0.0); - node_.param("/gazebo/gravity_z",g[2],-9.8); - - if((idsolver=new KDL::ChainIdSolver_RNE(chain,g)) == NULL) - { - ROS_ERROR("Failed to create ChainIDSolver_RNE."); - return false; - } - - // get wheelBase from URDF (actually from KDL tree) - KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support"); - KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip(); - - segmentMapIter=tree.getSegment("right_wheel_support"); - KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip(); - - wheelRadius.resize(chain.getNrOfJoints()); - wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3); - - // get wheelRadius from URDF (actually from KDL tree) - segmentMapIter=tree.getSegment("chassis"); - KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip(); - - segmentMapIter=tree.getSegment("left_wheel"); - KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint(); - wheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z(); - - segmentMapIter=tree.getSegment("right_wheel"); - KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); - wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); - - // set vectors to the right size - phi.resize(chain.getNrOfJoints()); - nu.resize(chain.getNrOfJoints()); - dnu.resize(chain.getNrOfJoints()); - torque.resize(chain.getNrOfJoints()); - - fext.resize(chain.getNrOfSegments()); - - return true; - } - - void CartLinearizingController_RNE::starting(const ros::Time& time) - { - for(unsigned int i=0; i < joints_.size();i++) v[i]=0.0; - } - - void CartLinearizingController_RNE::update(const ros::Time& time, - const ros::Duration& duration) - { - for(unsigned int i=0;i < joints_.size();i++) - { - phi(i)=joints_[i].getPosition(); - nu(i)=joints_[i].getVelocity(); - } - - - dnu(0)=v[0]/wheelRadius[0]-v[1]*wheelBase/2.0/wheelRadius[0]; // left wheel - dnu(1)=v[0]/wheelRadius[1]+v[1]*wheelBase/2.0/wheelRadius[1]; // right wheel - - - for(unsigned int i=0;i < fext.size();i++) fext[i].Zero(); - -// for(unsigned int i=0;i < joints_.size();i++) -// std::cout << "phi=" << phi(i) << " nu=" << nu(i) -// << " dnu=" << dnu(i) << std::endl; - - // Compute linearization. - if(idsolver->CartToJnt(phi,nu,dnu,fext,torque) < 0) ROS_ERROR("KDL inverse dynamics solver failed."); - -// for(unsigned int i=0;i < joints_.size(); i++) -// std::cout << "torque=" << torque(i) << std::endl; - - // Apply torques - for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i)); - } - - void CartLinearizingController_RNE::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command) - { - for(unsigned int i=0;i < command->data.size();i++) v[i]=command->data[i]; - } -} -PLUGINLIB_EXPORT_CLASS(twil_controllers::CartLinearizingController_RNE,controller_interface::ControllerBase) diff --git a/twil_controllers/src/nonsmooth_backstep_controller.cpp b/twil_controllers/src/nonsmooth_backstep_controller.cpp deleted file mode 100644 index a5efb91..0000000 --- a/twil_controllers/src/nonsmooth_backstep_controller.cpp +++ /dev/null @@ -1,243 +0,0 @@ -#include -#include - -#include -#include - -#define sqr(x) (x*x) -#define cub(x) (x*x*x) - -#define NUMERICAL_DETA - -namespace twil_controllers -{ - NonSmoothBackstepController::NonSmoothBackstepController(void): - wheelRadius(2) - { - } - - NonSmoothBackstepController::~NonSmoothBackstepController(void) - { - sub_command_.shutdown(); - } - - bool NonSmoothBackstepController::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n) - { - node_=n; - robot_=robot; - - XmlRpc::XmlRpcValue joint_names; - if(!node_.getParam("joints",joint_names)) - { - ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) - { - ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - for(int i=0; i < joint_names.size();i++) - { - XmlRpc::XmlRpcValue &name_value=joint_names[i]; - if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString) - { - ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str()); - return false; - } - - hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); - joints_.push_back(j); - } - sub_command_=node_.subscribe("command",1000,&NonSmoothBackstepController::commandCB,this); - sub_parameters_=node_.subscribe("dynamic_parameters",1000,&NonSmoothBackstepController::parametersCB,this); - - std::string robot_desc_string; - if(!node_.getParam("/robot_description",robot_desc_string)) - { - ROS_ERROR("Could not find '/robot_description'."); - return false; - } - - KDL::Tree tree; - if (!kdl_parser::treeFromString(robot_desc_string,tree)) - { - ROS_ERROR("Failed to construct KDL tree."); - return false; - } - - // get wheelBase from URDF (actually from KDL tree) - KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support"); - KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip(); - - segmentMapIter=tree.getSegment("right_wheel_support"); - KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip(); - - wheelRadius.resize(joints_.size()); - wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3); - - // get wheelRadius from URDF (actually from KDL tree) - segmentMapIter=tree.getSegment("chassis"); - KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip(); - - segmentMapIter=tree.getSegment("left_wheel"); - KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint(); - wheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z(); - - segmentMapIter=tree.getSegment("right_wheel"); - KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); - wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); - - const double K5 = 0.0018533548425194695; - const double K6 = 0.09946140462774823; - const double K7 = 21.65458426501294; - const double K8 = -15.40102896939387; - Ginv << 0.5*K7, 0.5*K8, - 0.5*K7, -0.5*K8; - F << 0.0, K5, - K6, 0.0; - - return true; - } - - void NonSmoothBackstepController::starting(const ros::Time& time) - { - xi.setZero(); - xiRef.setZero(); - eta.setZero(); - - } - - void NonSmoothBackstepController::update(const ros::Time& time, - const ros::Duration& duration) - { - if(duration.toSec() < 0.01) return; - - Eigen::Vector2d nu; - for(unsigned int i=0;i < joints_.size();i++) - { - nu[i]=joints_[i].getVelocity(); - } - - Eigen::Vector2d u; - u[0]=(nu[0]*wheelRadius[0]+nu[1]*wheelRadius[1])/2.0; - u[1]=(nu[1]*wheelRadius[1]-nu[0]*wheelRadius[0])/wheelBase; - - // Estimate pose by odometry - Eigen::MatrixXd B(3,2); - B << cos(xi[2]+u[1]*duration.toSec()/2.0), 0.0, - sin(xi[2]+u[1]*duration.toSec()/2.0), 0.0, - 0.0, 1.0; - - xi+=B*u*duration.toSec(); - - // Change of coordinates - Eigen::Matrix3d R; - R << cos(xiRef[2]), sin(xiRef[2]), 0.0, - -sin(xiRef[2]), cos(xiRef[2]), 0.0, - 0.0, 0.0, 1.0; - Eigen::Vector3d xBar=R*(xi-xiRef); - - // Discontinuous transformation - double e=sqrt(sqr(xBar[0])+sqr(xBar[1])); - double psi=atan2(xBar[1],xBar[0]); - double alpha=xBar[2]-psi; - - // Backstep controller - const double GAMMA1=10.0; - const double GAMMA2=1.0; -// const double GAMMA3=1.0; - const double GAMMA4=10.0; - const double GAMMA5=1.0; - const double LAMBDA1=1.0; - const double LAMBDA2=0.1; - const double LAMBDA3=0.1; - const double LAMBDA4=10.0; - const double LAMBDA5=100.0; - -#ifdef NUMERICAL_DETA - Eigen::Vector2d deta=-eta; -#endif - Eigen::Vector2d eta; - - eta[0]=-GAMMA1*e*cos(alpha); - - if(alpha > 1e-6) eta[1]=-GAMMA2*alpha-GAMMA1*sin(alpha)*cos(alpha)+GAMMA1*LAMBDA3*psi*sin(alpha)/LAMBDA2/alpha*cos(alpha); - else eta[1]=GAMMA1*LAMBDA3*psi/LAMBDA2; - - Eigen::Vector2d eb=u-eta; - - Eigen::Vector2d vBar; - if(e > 1e-6) vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha) - +LAMBDA2/LAMBDA4*alpha*sin(alpha)/e - -LAMBDA3/LAMBDA4*psi*sin(alpha)/e; - else vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha); - vBar[1]=-GAMMA5*eb[1]-LAMBDA2/LAMBDA5*alpha; - -#ifndef NUMERICAL_DETA - // Analitic derivates - Eigen::Vector2d deta; - deta[0]=sqr(GAMMA1)*e*cub(cos(alpha))-GAMMA1*GAMMA2*e*alpha*sin(alpha) - +sqr(GAMMA1)*LAMBDA3/LAMBDA2*e*cos(alpha)*sqr(sin(alpha))/alpha*psi; - deta[1]=sqr(GAMMA2)*alpha-2.0*GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi*sin(alpha)/alpha*cos(alpha) - +GAMMA1*GAMMA2*alpha*sqr(cos(alpha))-sqr(GAMMA1)*LAMBDA3/LAMBDA2*cub(cos(alpha))*sin(alpha)/alpha*psi - -GAMMA1*GAMMA2*alpha*sqr(sin(alpha))+sqr(GAMMA1)*LAMBDA3/LAMBDA2*cos(alpha)*cub(sin(alpha))/alpha*psi - -sqr(GAMMA1)*LAMBDA3/LAMBDA2*sqr(cos(alpha))*sqr(sin(alpha))/alpha-GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi - +sqr(GAMMA1*LAMBDA3/LAMBDA2)*cub(cos(alpha))*sin(alpha)/sqr(alpha)*sqr(psi)+sqr(GAMMA1*LAMBDA3/LAMBDA2)*cos(alpha)*cub(sin(alpha))/sqr(alpha)*sqr(psi) - +sqr(GAMMA1*LAMBDA3/LAMBDA2*cos(alpha)*sin(alpha)*psi)/cub(alpha); -#else - deta+=eta; - deta/=duration.toSec(); -#endif - - Eigen::Vector2d v=vBar+deta; - - - // Linearization. - Eigen::Vector2d uf; - uf << u[0]*u[1], sqr(u[1]); - - Eigen::Vector2d torque=Ginv*(v+F*uf); - - // Apply torques - for(unsigned int i=0;i < joints_.size();i++) - { - if(torque[i] > 100.0) torque[i]=100.0; - if(torque[i] < -100.0) torque[i]=-100.0; - joints_[i].setCommand(torque[i]); - } - } - - void NonSmoothBackstepController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command) - { - for(unsigned int i=0;i < command->data.size() && i < 2;i++) xiRef[i]=command->data[i]; - } - - void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command) - { - // data[0]=K5, data[1]=K6, data[2]=K7, data[3]=K8 - // data[4]=Pk5, data[5]=Pk6, data[6]=Pk7, data[7]=Pk8 - // F= [0 K5 - // K6 0] - // Ginv=[0.5K7 0.5K8 - // 0.5K7 -0.5K8 - - if(command->data[4] < 1e-3) F(0,1)=command->data[0]; - if(command->data[5] < 1e-3) F(1,0)=command->data[1]; - if(command->data[6] < 1e-3) - { - Ginv(0,0)=0.5*command->data[2]; - Ginv(1,0)=0.5*command->data[2]; - } - if(command->data[7] < 1e-3) - { - Ginv(0,1)=0.5*command->data[3]; - Ginv(1,1)=-0.5*command->data[3]; - } - } - -} - -PLUGINLIB_EXPORT_CLASS(twil_controllers::NonSmoothBackstepController,controller_interface::ControllerBase) diff --git a/twil_controllers/twil_controllers_plugins.xml b/twil_controllers/twil_controllers_plugins.xml index 15684e6..a2346e0 100644 --- a/twil_controllers/twil_controllers_plugins.xml +++ b/twil_controllers/twil_controllers_plugins.xml @@ -8,24 +8,4 @@ - - - The CartLinearizingController linearizes the Twil dynamic model. The - linearized inputs are linear and angular accelerations. It expects a - EffortJointInterface type of hardware interface. Recursive - Newton-Euler from KDL is used to compute torque. Not working - properly. - - - - - - The NonSmoothBackstepController implements a non-smooth controller - based on a discontinuous transformation and backstepping. A feedback - linearization of the Twil dynamic model is also performed. The - inputs to the controller are the reference Cartesian pose. It expects a - EffortJointInterface type of hardware interface. - - - diff --git a/twil_description/doc/dimensions_battery_bosch_12v.pdf b/twil_description/doc/dimensions_battery_bosch_12v.pdf deleted file mode 100644 index 2184e85..0000000 Binary files a/twil_description/doc/dimensions_battery_bosch_12v.pdf and /dev/null differ diff --git a/twil_description/doc/dimensions_chassis.pdf b/twil_description/doc/dimensions_chassis.pdf deleted file mode 100644 index 114b93c..0000000 Binary files a/twil_description/doc/dimensions_chassis.pdf and /dev/null differ diff --git a/twil_description/doc/dimensions_cpu.pdf b/twil_description/doc/dimensions_cpu.pdf deleted file mode 100644 index d3488b0..0000000 Binary files a/twil_description/doc/dimensions_cpu.pdf and /dev/null differ diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro index ebc8381..bd22a0b 100644 --- a/twil_description/xacro/twil.urdf.xacro +++ b/twil_description/xacro/twil.urdf.xacro @@ -98,13 +98,6 @@ /twil - - - - - - - 0.001 diff --git a/twil_ident/launch/ident.launch b/twil_ident/launch/ident.launch index 002625c..672ea74 100644 --- a/twil_ident/launch/ident.launch +++ b/twil_ident/launch/ident.launch @@ -1,17 +1,20 @@ - - + + - + - - + + diff --git a/twil_ident/src/ident.cpp b/twil_ident/src/ident.cpp index 028497e..82d9d61 100644 --- a/twil_ident/src/ident.cpp +++ b/twil_ident/src/ident.cpp @@ -47,7 +47,6 @@ Prbs::operator int(void) return sh[index]; } - class Ident { public: @@ -72,7 +71,6 @@ class Ident Eigen::MatrixXd P2; std::vector prbs; - int iter; ros::Time lastTime; @@ -80,7 +78,6 @@ class Ident void resetCovariance(void); }; - Ident::Ident(ros::NodeHandle node): nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),prbs(nJoints) { @@ -110,7 +107,6 @@ void Ident::resetCovariance(void) P1*=1; P2.setIdentity(); P2*=1; - iter=0; } void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) @@ -160,8 +156,6 @@ void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) dynParam.data.push_back(P2(i,i)); } dynParamPublisher.publish(dynParam); - -// if(iter++ > 2048) resetCovariance(); } void Ident::setCommand(void)