From: Walter Fetter Lages Date: Wed, 21 Mar 2018 19:59:45 +0000 (-0300) Subject: ROSBook 2018 initial submision X-Git-Tag: rosbook2018initial^0 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=05f30eea05aab508d73391d49f0e8a19b43af400;p=twil.git ROSBook 2018 initial submision twil_description: remove doc twil_controllers: remove car_linearizing_rne remove adaptive_nonsmooth_backstep remove numerical derivates --- diff --git a/twil_controllers/README b/twil_controllers/README deleted file mode 100644 index 7c75eb7..0000000 --- a/twil_controllers/README +++ /dev/null @@ -1,15 +0,0 @@ -To publish with joint_effort_controller: - -rostopic pub /twil/left_wheel_joint_effort_controller/command std_msgs/Float64 "0.0" - -To publish with cart_linearizing_controller: - -rostopic pub /twil/cart_linearizing_controller/command std_msgs/Float64MultiArray "data: [0.1, 0.0]" - -To publish with nonsmmoth_backstep_controller: - -rostopic pub /twil/nonsmooth_backstep_controller/command geometry_ms/Pose2D '{x: 0.0, y: 0.0, theta: 0.0}' - -To get time and pose: - -rostopic echo -p /gazebo/model_states | awk '{FS=","; printf("%g %g %g %g\n",$1/1e9,$11,$12,2*atan2(sqrt($14^2+$15^2+$16^2),$17))}' diff --git a/twil_controllers/config/nonsmooth_backstep_control.yaml b/twil_controllers/config/nonsmooth_backstep_control.yaml index afcc9f5..6ff08e6 100644 --- a/twil_controllers/config/nonsmooth_backstep_control.yaml +++ b/twil_controllers/config/nonsmooth_backstep_control.yaml @@ -1,4 +1,4 @@ -# Watch-out: The indenting here is relevant to the semantic! +# Watch-out: The indentation here is relevant to the semantic! twil: 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 index c00b0f2..f37692e 100644 --- a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h +++ b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h @@ -23,49 +23,55 @@ namespace twil_controllers { - class NonSmoothBackstepController: public controller_interface::Controller + class NonSmoothBackstepController: public controller_interface:: + Controller { public: - NonSmoothBackstepController(void); - ~NonSmoothBackstepController(void); + 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); + 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::NodeHandle node_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; - boost::scoped_ptr - > status_publisher_ ; + boost::scoped_ptr + > status_publisher_ ; - boost::shared_ptr > odom_publisher_; - boost::shared_ptr > tf_odom_publisher_; + boost::shared_ptr > odom_publisher_; + boost::shared_ptr > tf_odom_publisher_; - ros::Subscriber sub_command_; - ros::Subscriber sub_parameters_; + ros::Subscriber sub_command_; + ros::Subscriber sub_parameters_; - Eigen::Matrix2d Ginv; - Eigen::Matrix2d F; + Eigen::Matrix2d Ginv; + Eigen::Matrix2d F; - std::vector wheelRadius; - double wheelBase; + std::vector wheelRadius; + double wheelBase; - Eigen::Vector3d x; - Eigen::Vector3d xRef; + Eigen::Vector3d x; + Eigen::Vector3d xRef; - Eigen::Vector2d eta; + Eigen::Vector2d eta; - ros::Time lastSamplingTime; + ros::Time lastSamplingTime; - std::vector lambda; - std::vector gamma; + Eigen::Vector2d phi; - void commandCB(const geometry_msgs::Pose2D::ConstPtr &command); - void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command); + std::vector lambda; + std::vector gamma; + + void commandCB(const geometry_msgs::Pose2D::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 ac31acb..0000000 --- a/twil_controllers/launch/adaptive_nonsmooth_backstep.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - diff --git a/twil_controllers/launch/gazebo.launch b/twil_controllers/launch/gazebo_joint_effort.launch similarity index 100% rename from twil_controllers/launch/gazebo.launch rename to twil_controllers/launch/gazebo_joint_effort.launch diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch b/twil_controllers/launch/gazebo_nonsmooth_backstep.launch index 45e7518..16002aa 100644 --- a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch +++ b/twil_controllers/launch/gazebo_nonsmooth_backstep.launch @@ -4,8 +4,7 @@ - - + @@ -14,14 +13,6 @@ - - - - - [B - - - - + diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch b/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch new file mode 100644 index 0000000..6375e93 --- /dev/null +++ b/twil_controllers/launch/gazebo_nonsmooth_backstep8.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_controllers/scripts/pose_step.py b/twil_controllers/scripts/pose_step.py deleted file mode 100755 index f0c772e..0000000 --- a/twil_controllers/scripts/pose_step.py +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/python - -import sys -import time - -import rospy -from geometry_msgs.msg import Pose2D - -def step(): - if len(sys.argv) < 4: - print 'pose_step.py x y theta' - exit() - pose=Pose2D() - pose.x=float(sys.argv[1]) - pose.y=float(sys.argv[2]) - pose.theta=float(sys.argv[3]) - - pub=rospy.Publisher('/twil/nonsmooth_backstep_controller/command', Pose2D, queue_size=100) - - rospy.init_node('pose_step',anonymous=True) - - pub.publish(pose) - time.sleep(1) - -if __name__ == '__main__': - try: - step() - except rospy.ROSInterruptException: - pass 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 index 5abdcd7..626d207 100644 --- a/twil_controllers/src/nonsmooth_backstep_controller.cpp +++ b/twil_controllers/src/nonsmooth_backstep_controller.cpp @@ -9,10 +9,6 @@ #define sqr(x) (x*x) #define cub(x) (x*x*x) -// Are numeric derivates of eta too noisy? -// It is not simple to compute it analitically... -#define NUMERICAL_DETA - namespace twil_controllers { NonSmoothBackstepController::NonSmoothBackstepController(void): @@ -139,17 +135,17 @@ namespace twil_controllers KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); - gamma[0]=1; - gamma[1]=1; - gamma[2]=1; - gamma[3]=1; + gamma[0]=10.0; + gamma[1]=1.0; + gamma[2]=10.0; + gamma[3]=10.0; node_.param("gamma",gamma,gamma); - lambda[0]=1; - lambda[1]=1; - lambda[2]=1; - lambda[3]=1; - lambda[4]=1; + lambda[0]=10.0; + lambda[1]=0.1; + lambda[2]=0.1; + lambda[3]=50.0; + lambda[4]=100.0; node_.param("lambda",lambda,lambda); const double K5=0.08444758509282763; @@ -171,6 +167,10 @@ namespace twil_controllers xRef.setZero(); eta.setZero(); lastSamplingTime=time; + for(unsigned int i=0;i < joints_.size();i++) + { + phi[i]=joints_[i].getPosition(); + } } void NonSmoothBackstepController::update(const ros::Time& time, @@ -181,31 +181,34 @@ namespace twil_controllers if(fabs(dt-0.01) > 0.001/2) return; lastSamplingTime=time; - Eigen::Vector2d w; + // Incremental encoders sense angular displacement and + // not velocity + // phi[0] is the left wheel angular displacement + // phi[1] is the right wheel angular displacement + Eigen::Vector2d deltaPhi=-phi; for(unsigned int i=0;i < joints_.size();i++) { - w[i]=joints_[i].getVelocity(); + phi[i]=joints_[i].getPosition(); } + deltaPhi+=phi; - // w[0] is the left wheel velocity - // w[1] is the right wheel velocity - Eigen::Vector2d u; - u[0]=(w[0]*wheelRadius[0]+w[1]*wheelRadius[1])/2.0; - u[1]=(w[1]*wheelRadius[1]-w[0]*wheelRadius[0])/wheelBase; - // Estimate pose by odometry - Eigen::MatrixXd B(3,2); - double deltaTheta2=u[1]*dt/2.0; - double deltaS=(fabs(deltaTheta2) > 1e-10)? sin(deltaTheta2)/deltaTheta2:1.0; + Eigen::Vector2d deltaU; + deltaU << (deltaPhi[0]*wheelRadius[0]+deltaPhi[1]*wheelRadius[1])/2.0, + (deltaPhi[1]*wheelRadius[1]-deltaPhi[0]*wheelRadius[0])/wheelBase; + + double deltaS=(fabs(deltaU[1]) > 1e-10)? + deltaU[0]*sin(deltaU[1])/deltaU[1]:deltaU[0]; - B << cos(x[2]+deltaS*deltaTheta2), 0.0, - sin(x[2]+deltaS*deltaTheta2), 0.0, - 0.0, 1.0; + Eigen::Vector3d deltaX; + deltaX << deltaS*cos(x[2]+deltaU[1]/2.0), + deltaS*sin(x[2]+deltaU[1]/2.0), + deltaU[1]; - Eigen::Vector3d dx=B*u; - - x+=dx*dt; + x+=deltaX; + Eigen::Vector2d u=deltaU/dt; + // Change of coordinates Eigen::Matrix3d R; R << cos(xRef[2]), sin(xRef[2]), 0.0, @@ -218,12 +221,13 @@ namespace twil_controllers double psi=atan2(xBar[1],xBar[0]); double alpha=xBar[2]-psi; -#ifdef NUMERICAL_DETA - Eigen::Vector2d deta=-eta; -#endif + // Backstepping eta[0]=-gamma[0]*e*cos(alpha); - if(fabs(alpha ) > 1e-10) eta[1]=-gamma[1]*alpha-gamma[0]*sin(alpha)*cos(alpha)+gamma[0]*lambda[2]*psi*sin(alpha)/lambda[1]/alpha*cos(alpha); + if(fabs(alpha ) > 1e-10) + eta[1]=-gamma[1]*alpha + -gamma[0]*sin(alpha)*cos(alpha)+gamma[0]*lambda[2]*psi*sin(alpha)/ + lambda[1]/alpha*cos(alpha); else eta[1]=gamma[0]*lambda[2]*psi/lambda[1]; Eigen::Vector2d eb=u-eta; @@ -235,26 +239,31 @@ namespace twil_controllers else vBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha); vBar[1]=-gamma[3]*eb[1]-lambda[1]/lambda[4]*alpha; -#ifdef NUMERICAL_DETA - // Numerical derivates - deta+=eta; - deta/=dt; -#else - // Analitic derivates Eigen::Vector2d deta; - deta[0]=sqr(gamma[0])*e*cub(cos(alpha))-gamma[0]*gamma[1]*e*alpha*sin(alpha) - +sqr(gamma[0])*lambda[2]/lambda[1]*e*cos(alpha)*sqr(sin(alpha))/alpha*psi; - deta[1]=sqr(gamma[1])*alpha-2.0*gamma[0]*gamma[1]*lambda[2]/lambda[1]*psi*sin(alpha)/alpha*cos(alpha) - +gamma[0]*gamma[1]*alpha*sqr(cos(alpha))-sqr(gamma[0])*lambda[2]/lambda[1]*cub(cos(alpha))*sin(alpha)/alpha*psi - -gamma[0]*gamma[1]*alpha*sqr(sin(alpha))+sqr(gamma[0])*lambda[2]/lambda[1]*cos(alpha)*cub(sin(alpha))/alpha*psi - -sqr(gamma[0])*lambda[2]/lambda[1]*sqr(cos(alpha))*sqr(sin(alpha))/alpha-gamma[0]*gamma[1]*lambda[2]/lambda[1]*psi - +sqr(gamma[0]*lambda[2]/lambda[1])*cub(cos(alpha))*sin(alpha)/sqr(alpha)*sqr(psi)+sqr(gamma[0]*lambda[2]/lambda[1])*cos(alpha)*cub(sin(alpha))/sqr(alpha)*sqr(psi) - +sqr(gamma[0]*lambda[2]/lambda[1]*cos(alpha)*sin(alpha)*psi)/cub(alpha); -#endif + + deta[0]=e*cub(cos(alpha))*sqr(gamma[0])-e*gamma[0]*sin(alpha)*(alpha*gamma[1] + -(gamma[0]*lambda[2]*cos(alpha)*sin(alpha)*psi)/(alpha*lambda[1])); + + deta[1]=gamma[1]*(alpha*gamma[1] + -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) + +gamma[0]*sqr(cos(alpha))*(alpha*gamma[1] + -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) + -gamma[0]*sqr(sin(alpha))*(alpha*gamma[1] + -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1])) + -(lambda[2]*sqr(cos(alpha))*sqr(sin(alpha))*sqr(gamma[0]))/(alpha*lambda[1]) + -(gamma[0]*lambda[2]*psi*sqr(cos(alpha))*(alpha*gamma[1] + -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) + /(alpha*lambda[1]) + +(gamma[0]*lambda[2]*psi*sqr(sin(alpha))*(alpha*gamma[1] + -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) + /(alpha*lambda[1]) + +(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha)*(alpha*gamma[1] + -(gamma[0]*lambda[2]*psi*cos(alpha)*sin(alpha))/(alpha*lambda[1]))) + /(sqr(alpha)*lambda[1]); Eigen::Vector2d v=vBar+deta; - // Linearization. + // Linearization Eigen::Vector2d uf; uf << u[0]*u[1], sqr(u[1]); @@ -277,10 +286,10 @@ namespace twil_controllers status_publisher_->msg_.process_value.x=x[0]; status_publisher_->msg_.process_value.y=x[1]; status_publisher_->msg_.process_value.theta=x[2]; - - status_publisher_->msg_.process_value_dot.x=dx[0]; - status_publisher_->msg_.process_value_dot.y=dx[1]; - status_publisher_->msg_.process_value_dot.theta=dx[2]; + + status_publisher_->msg_.process_value_dot.x=deltaX[0]/dt; + status_publisher_->msg_.process_value_dot.y=deltaX[1]/dt; + status_publisher_->msg_.process_value_dot.theta=deltaX[2]/dt; status_publisher_->msg_.error.x=xRef[0]-x[0]; status_publisher_->msg_.error.y=xRef[1]-x[1]; @@ -331,16 +340,17 @@ namespace twil_controllers odom_publisher_->msg_.pose.pose.orientation.z=sin(x[2]/2); odom_publisher_->msg_.pose.pose.orientation.w=cos(x[2]/2); - odom_publisher_->msg_.twist.twist.linear.x=dx[0]; - odom_publisher_->msg_.twist.twist.linear.y=dx[1]; - odom_publisher_->msg_.twist.twist.angular.z=dx[2]; + odom_publisher_->msg_.twist.twist.linear.x=deltaX[0]/dt; + odom_publisher_->msg_.twist.twist.linear.y=deltaX[1]/dt; + odom_publisher_->msg_.twist.twist.angular.z=deltaX[2]/dt; odom_publisher_->unlockAndPublish(); } if(tf_odom_publisher_ && tf_odom_publisher_->trylock()) { - geometry_msgs::TransformStamped& odom_frame=tf_odom_publisher_->msg_.transforms[0]; + geometry_msgs::TransformStamped &odom_frame= + tf_odom_publisher_->msg_.transforms[0]; odom_frame.header.stamp=time; odom_frame.transform.translation.x=x[0]; odom_frame.transform.translation.y=x[1]; diff --git a/twil_description/doc/bouncing.txt b/twil_description/doc/bouncing.txt deleted file mode 100644 index 65b6eff..0000000 --- a/twil_description/doc/bouncing.txt +++ /dev/null @@ -1,47 +0,0 @@ -here are a couple of sources of undesired "bouncing behavior" that I've -commonly seen with vehicles: - - cause: interpenetration correction applies an impulse that pushes the -object away from interpenetrating collision, but if the impulse is too large -it causes the object to "bounce/fly away". fix: set maximum velocity -impulse (max_vel) to 0 will enforce pure position correction with no added -momentum from interpenetration correction. It is recommended to set a -finite minimum allowable interpenetration depth (min_depth) of 1mm or -something small in comparison to the colliding objects. - - cause: controller exerts unrealistically large efforts on joints or -links. fix: double check the forces/torques commanded by your controller -and make sure they are physically realistic if you are looking for -physically realistic responses. - - cause: unrealistically high velocity / momentum buildup. fix: add -dissipation by adding joint damping and set implicit_spring_damper flag to -true to avoid numerical instability. Joint damping value is in the units of -(effort / velocity). One extremely simplistic way to try and come up with -viscous damping estimates is this: at what joint velocity do you want -significant effort opposing your actuation forces? Assuming for example, we -are controlling a hinged joint, and the actuator is capable of putting out 1 -Nm of torque. A damping coefficient of 2 means the damping force opposing -your actuator will neutralize actuation torque of 1 Nm when the joint is -rotating at 0.5 rad/s. - - cause: friction coefficients are too large, combined with failure of -dynamics solver to resolve all constraints sufficiently in allotted time. -This is especially true of you have opposing frictional forces at work such -as in the cases of skid-steer or non-ackerman steering. fix: use friction -coefficient of 1 or lower, and potentially increase number of inner -iterations. - -Here's an example for specifying contact parameters discussed above in URDF -as Gazebo extensions, this example applies the parameters to all collision -shapes specified under a link named my_link: - - - 1000000.0 - 1.0 - 0.8 - 0.8 - 0.0 - 0.001 - - 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_gazebo_ros_control/CMakeLists.txt b/twil_gazebo_ros_control/CMakeLists.txt deleted file mode 100644 index 2829f2f..0000000 --- a/twil_gazebo_ros_control/CMakeLists.txt +++ /dev/null @@ -1,165 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(twil_gazebo_ros_control) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - gazebo - gazebo_ros_control - twil_description -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES twil_gazebo_ros_control - CATKIN_DEPENDS controller_manager pluginlib gazebo_ros_control twil_description -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories(include - ${GAZEBO_INCLUDE_DIRS} -) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -add_library(twil_gazebo_ros_control - src/twil_robot_hw_sim.cpp -) - -## Declare a cpp executable -# add_executable(twil_gazebo_ros_control_node src/twil_gazebo_ros_control_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(twil_gazebo_ros_control_node twil_gazebo_ros_control_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_gazebo_ros_control.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/twil_gazebo_ros_control/package.xml b/twil_gazebo_ros_control/package.xml deleted file mode 100644 index 6f2f997..0000000 --- a/twil_gazebo_ros_control/package.xml +++ /dev/null @@ -1,66 +0,0 @@ - - - twil_gazebo_ros_control - 2.0.0 - The twil_gazebo_ros_control package - - - - - Walter Fetter Lages - - - - - - GPLv3 - - - - - - - - - - - - - Walter Fetter Lages - - - - - - - - - - - - - - catkin - - gazebo - gazebo_ros_control - controller_manager - pluginlib - twil_description - - controller_manager - pluginlib - - gazebo_ros_control - twil_description - - - - - - - - - - - \ No newline at end of file diff --git a/twil_gazebo_ros_control/src/twil_robot_hw_sim.cpp b/twil_gazebo_ros_control/src/twil_robot_hw_sim.cpp deleted file mode 100644 index a21f668..0000000 --- a/twil_gazebo_ros_control/src/twil_robot_hw_sim.cpp +++ /dev/null @@ -1,105 +0,0 @@ -#include - -#include -#include - -#include - -#include - -#include -#include -#include - -namespace twil_gazebo_ros_control -{ - - class TwilRobotHWSim:public gazebo_ros_control::RobotHWSim - { - - unsigned int n_dof_; - - hardware_interface::JointStateInterface js_interface_; - hardware_interface::EffortJointInterface ej_interface_; - - std::vector joint_name_; - std::vector joint_position_; - std::vector joint_velocity_; - std::vector joint_effort_; - std::vector joint_effort_command_; - - std::vector sim_joints_; - - public: - - TwilRobotHWSim(void):n_dof_(2),joint_name_(n_dof_),joint_position_(n_dof_), - joint_velocity_(n_dof_),joint_effort_(n_dof_),joint_effort_command_(n_dof_) - { - - joint_name_[0]="left_wheel_joint"; - joint_name_[1]="right_wheel_joint"; - - for(unsigned int j=0;j < n_dof_;j++) - { - joint_position_[j]=0.0; - joint_velocity_[j]=0.0; - joint_effort_[j]=0.0; - - joint_effort_command_[j] = 0.0; - - js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_name_[j],&joint_position_[j], - &joint_velocity_[j],&joint_effort_[j])); - - joint_effort_command_[j] = 0.0; - ej_interface_.registerHandle(hardware_interface::JointHandle(js_interface_.getHandle(joint_name_[j]), - &joint_effort_command_[j])); - - } - - registerInterface(&js_interface_); - registerInterface(&ej_interface_); - } - - - bool initSim(const std::string& robot_namespace, - ros::NodeHandle nh,gazebo::physics::ModelPtr model, - const urdf::Model *const urdf_model, - std::vector transmissions) - { - for(unsigned int j=0;j < n_dof_;j++) - { - ROS_INFO_STREAM("Getting pointer to gazebo joint: " << joint_name_[j]); - gazebo::physics::JointPtr joint=model->GetJoint(joint_name_[j]); - if(joint) sim_joints_.push_back(joint); - else - { - ROS_ERROR_STREAM("This robot has a joint named \"" << joint_name_[j] - <<"\" which is not in the gazebo model."); - return false; - } - } - return true; - } - - void readSim(ros::Time time,ros::Duration period) - { - for(unsigned int j=0; j < n_dof_;j++) - { -// joint_position_[j]+=angles::shortest_angular_distance -// (joint_position_[j],sim_joints_[j]->GetAngle(0).GetAsRadian()); - joint_position_[j]=sim_joints_[j]->GetAngle(0).Radian(); - joint_velocity_[j]=sim_joints_[j]->GetVelocity(0); -// joint_effort_[j]=sim_joints_[j]->GetForce(0); - joint_effort_[j]=joint_effort_command_[j]; - } - } - - void writeSim(ros::Time time,ros::Duration period) - { - for(unsigned int j=0;j < n_dof_;j++) sim_joints_[j]->SetForce(0,joint_effort_command_[j]); - } - - }; -} - -PLUGINLIB_EXPORT_CLASS(twil_gazebo_ros_control::TwilRobotHWSim,gazebo_ros_control::RobotHWSim) diff --git a/twil_gazebo_ros_control/twil_gazebo_ros_control_plugins.xml b/twil_gazebo_ros_control/twil_gazebo_ros_control_plugins.xml deleted file mode 100644 index 7d03185..0000000 --- a/twil_gazebo_ros_control/twil_gazebo_ros_control_plugins.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - A ROS/Gazebo interface Twil, exporting a joint_state_interface and a - joint_effort_interface. - - - diff --git a/twil_ident/CMakeLists.txt b/twil_ident/CMakeLists.txt deleted file mode 100644 index 55300bb..0000000 --- a/twil_ident/CMakeLists.txt +++ /dev/null @@ -1,164 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(twil_ident) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp kdl_parser) - -find_package(cmake_modules REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) -find_package(Eigen REQUIRED) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES twil_ident -# CATKIN_DEPENDS other_catkin_pkg - DEPENDS eigen -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -# TODO: Check names of system library include directories (eigen) -include_directories( - ${Eigen_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(twil_ident -# src/${PROJECT_NAME}/twil_ident.cpp -# ) - -## Declare a cpp executable -add_executable(ident src/ident.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(twil_ident_node twil_ident_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -target_link_libraries(ident - ${catkin_LIBRARIES} - ${eigen_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -install(TARGETS ident -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_ident.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/twil_ident/launch/gazebo.launch b/twil_ident/launch/gazebo.launch deleted file mode 100644 index df6206f..0000000 --- a/twil_ident/launch/gazebo.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/twil_ident/launch/ident.launch b/twil_ident/launch/ident.launch deleted file mode 100644 index 3e6e69a..0000000 --- a/twil_ident/launch/ident.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/twil_ident/package.xml b/twil_ident/package.xml deleted file mode 100644 index ffefa1b..0000000 --- a/twil_ident/package.xml +++ /dev/null @@ -1,57 +0,0 @@ - - - twil_ident - 2.0.0 - The twil_ident package - - - - - Walter Fetter Lages - - - - - - GPLv3 - - - - - - - - - - - - - Walter Fetter Lages - - - - - - - - - - - - - - catkin - eigen - kdl_parser - eigen - kdl_parser - - - - - - - - - - diff --git a/twil_ident/src/ident.cpp b/twil_ident/src/ident.cpp deleted file mode 100644 index c1ffaf5..0000000 --- a/twil_ident/src/ident.cpp +++ /dev/null @@ -1,242 +0,0 @@ -#include - -#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; - - std::vector wheelRadius; - double wheelBase; - - 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),wheelRadius(nJoints) -{ - node_=node; - - std::string robot_desc_string; - if(!node_.getParam("/robot_description",robot_desc_string)) - { - ROS_ERROR("Could not find '/robot_description'."); - } - - KDL::Tree tree; - if (!kdl_parser::treeFromString(robot_desc_string,tree)) - { - ROS_ERROR("Failed to construct KDL tree."); - } - - // 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(nJoints); - 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(); - - 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); - - // u(k+1) - // jointStates->velocity[0] is left wheel - // jointStates->velocity[1] is right wheel - u[0]=(jointStates->velocity[0]*wheelRadius[0]+jointStates->velocity[1]*wheelRadius[1])/2; - u[1]=(jointStates->velocity[1]*wheelRadius[1]-jointStates->velocity[0]*wheelRadius[0])/wheelBase; - - for(int i=0;i < nJoints;i++) - 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; - dynParam.layout.dim.push_back(std_msgs::MultiArrayDimension()); - dynParam.layout.dim[0].label="K5 K6 K7 K8 P55 P66 P77 P88"; - dynParam.layout.dim[0].size=nJoints*4; - dynParam.layout.dim[0].stride=1; - dynParam.layout.data_offset=0; - 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; -}