From: Walter Fetter Lages Date: Wed, 21 Mar 2018 18:09:13 +0000 (-0300) Subject: add twil_trajectories X-Git-Tag: rosbook2018initial~1 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=aee27b271eaa6ff2042212769773ea9a63239c81;p=twil.git add twil_trajectories twil_description: add doc use arguments to launch twil or twil/wam add support to rviz add effort limit to fixed wheels twil_ident: get geometic parameters from parameter server twil_controllers: nonsmooth_backstep_controller: parameter read form yaml file publish odometry and status --- diff --git a/twil/package.xml b/twil/package.xml index 3ccfd15..5d03e91 100644 --- a/twil/package.xml +++ b/twil/package.xml @@ -42,10 +42,14 @@ catkin twil_description + twil_controllers - + twil_ident + @@ -56,4 +60,4 @@ - \ No newline at end of file + diff --git a/twil_controllers/CMakeLists.txt b/twil_controllers/CMakeLists.txt index 7261bb6..5fd9edb 100644 --- a/twil_controllers/CMakeLists.txt +++ b/twil_controllers/CMakeLists.txt @@ -5,8 +5,14 @@ project(twil_controllers) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + roscpp controller_interface effort_controllers + kdl_parser + geometry_msgs + nav_msgs + tf + message_generation ) find_package(cmake_modules REQUIRED) @@ -14,6 +20,7 @@ find_package(cmake_modules REQUIRED) # 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 @@ -44,11 +51,13 @@ find_package(Eigen REQUIRED) ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES +add_message_files( + FILES # Message1.msg # Message2.msg -# ) + NonSmoothBackstepControllerStatus.msg + PosePolar.msg +) ## Generate services in the 'srv' folder # add_service_files( @@ -65,10 +74,12 @@ find_package(Eigen REQUIRED) # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs + geometry_msgs + nav_msgs +) ################################### ## catkin specific configuration ## @@ -80,11 +91,10 @@ find_package(Eigen REQUIRED) ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include + INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS controller_interface + CATKIN_DEPENDS message_runtime # DEPENDS system_lib - DEPENDS eigen ) ########### @@ -110,7 +120,7 @@ add_library(twil_controllers ## Add cmake target dependencies of the executable/library ## as an example, message headers may need to be generated before nodes -# add_dependencies(twil_controllers_node twil_controllers_generate_messages_cpp) +add_dependencies(twil_controllers twil_controllers_generate_messages_cpp) ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME} diff --git a/twil_controllers/README b/twil_controllers/README index acd2807..7c75eb7 100644 --- a/twil_controllers/README +++ b/twil_controllers/README @@ -6,6 +6,10 @@ 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 d5761f9..afcc9f5 100644 --- a/twil_controllers/config/nonsmooth_backstep_control.yaml +++ b/twil_controllers/config/nonsmooth_backstep_control.yaml @@ -1,3 +1,5 @@ +# Watch-out: The indenting here is relevant to the semantic! + twil: joint_state_controller: @@ -9,3 +11,5 @@ twil: joints: - left_wheel_joint - right_wheel_joint + lambda: [10.0, 0.1, 0.1, 50.0, 100.0] + gamma: [10.0, 1.0, 10.0, 10.0] diff --git a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h index 4b9c205..c00b0f2 100644 --- a/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h +++ b/twil_controllers/include/twil_controllers/nonsmooth_backstep_controller.h @@ -5,13 +5,21 @@ #include #include +#include +#include + #include + +#include +#include +#include +#include + #include #include -#include -#include +#include -#include +#include namespace twil_controllers { @@ -29,7 +37,14 @@ namespace twil_controllers ros::NodeHandle node_; hardware_interface::EffortJointInterface *robot_; std::vector joints_; - + + boost::scoped_ptr + > status_publisher_ ; + + boost::shared_ptr > odom_publisher_; + boost::shared_ptr > tf_odom_publisher_; + ros::Subscriber sub_command_; ros::Subscriber sub_parameters_; @@ -39,12 +54,17 @@ namespace twil_controllers std::vector wheelRadius; double wheelBase; - Eigen::Vector3d xi; - Eigen::Vector3d xiRef; + Eigen::Vector3d x; + Eigen::Vector3d xRef; Eigen::Vector2d eta; - void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command); + ros::Time lastSamplingTime; + + std::vector lambda; + std::vector gamma; + + void commandCB(const geometry_msgs::Pose2D::ConstPtr &command); void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command); }; } diff --git a/twil_controllers/launch/adaptive_nonsmooth_backstep.launch b/twil_controllers/launch/adaptive_nonsmooth_backstep.launch index fb8904e..ac31acb 100644 --- a/twil_controllers/launch/adaptive_nonsmooth_backstep.launch +++ b/twil_controllers/launch/adaptive_nonsmooth_backstep.launch @@ -1,21 +1,13 @@ - - - - - - + output="screen" ns="twil" args="joint_state_controller nonsmooth_backstep_controller"/> - + - - diff --git a/twil_controllers/launch/cart_linearizing.launch b/twil_controllers/launch/cart_linearizing.launch index 8e08ffe..f5089a8 100644 --- a/twil_controllers/launch/cart_linearizing.launch +++ b/twil_controllers/launch/cart_linearizing.launch @@ -1,16 +1,6 @@ - - - - - - - - - - + output="screen" ns="twil" args="joint_state_controller cart_linearizing_controller"/> diff --git a/twil_controllers/launch/gazebo.launch b/twil_controllers/launch/gazebo.launch new file mode 100644 index 0000000..67f846b --- /dev/null +++ b/twil_controllers/launch/gazebo.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/twil_controllers/launch/gazebo_nonsmooth_backstep.launch b/twil_controllers/launch/gazebo_nonsmooth_backstep.launch new file mode 100644 index 0000000..45e7518 --- /dev/null +++ b/twil_controllers/launch/gazebo_nonsmooth_backstep.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + [B + + + + + + diff --git a/twil_controllers/launch/joint_effort.launch b/twil_controllers/launch/joint_effort.launch index 0fe3514..222450c 100644 --- a/twil_controllers/launch/joint_effort.launch +++ b/twil_controllers/launch/joint_effort.launch @@ -1,16 +1,6 @@ - - - - - - - - - - + output="screen" ns="twil" args="left_wheel_joint_effort_controller right_wheel_joint_effort_controller joint_state_controller"/> diff --git a/twil_controllers/launch/joint_velocity.launch b/twil_controllers/launch/joint_velocity.launch index 4c7f96d..b93a84a 100644 --- a/twil_controllers/launch/joint_velocity.launch +++ b/twil_controllers/launch/joint_velocity.launch @@ -1,16 +1,6 @@ - - - - - - - - - - + output="screen" ns="twil" args="joint_state_controller left_wheel_joint_velocity_controller right_wheel_joint_velocity_controller"/> diff --git a/twil_controllers/launch/nonsmooth_backstep.launch b/twil_controllers/launch/nonsmooth_backstep.launch index f8f3a80..695b6e3 100644 --- a/twil_controllers/launch/nonsmooth_backstep.launch +++ b/twil_controllers/launch/nonsmooth_backstep.launch @@ -1,16 +1,6 @@ - - - - - - - - - - + output="screen" ns="twil" args="joint_state_controller nonsmooth_backstep_controller"/> diff --git a/twil_controllers/msg/NonSmoothBackstepControllerStatus.msg b/twil_controllers/msg/NonSmoothBackstepControllerStatus.msg new file mode 100644 index 0000000..3012416 --- /dev/null +++ b/twil_controllers/msg/NonSmoothBackstepControllerStatus.msg @@ -0,0 +1,16 @@ +Header header +geometry_msgs/Pose2D set_point +geometry_msgs/Pose2D process_value +geometry_msgs/Pose2D process_value_dot +geometry_msgs/Pose2D error +float64 time_step +float64[2] command +float64[5] lambda +float64[4] gamma +PosePolar polar_error +float64[2] backstep_set_point +float64[2] backstep_set_point_dot +float64[2] backstep_process_value +float64[2] backstep_error +float64[2] backstep_command +float64[2] linear_dynamics_command diff --git a/twil_controllers/msg/PosePolar.msg b/twil_controllers/msg/PosePolar.msg new file mode 100644 index 0000000..14a00fa --- /dev/null +++ b/twil_controllers/msg/PosePolar.msg @@ -0,0 +1,3 @@ +float64 range +float64 angle +float64 orientation diff --git a/twil_controllers/package.xml b/twil_controllers/package.xml index 81a7744..d1179cd 100644 --- a/twil_controllers/package.xml +++ b/twil_controllers/package.xml @@ -45,16 +45,24 @@ controller_interface effort_controllers kdl_parser - orocos_kdl - twil_gazebo_ros_control + geometry_msgs + nav_msgs + tf + control_toolbox + realtime_tools + message_generation controller_interface controller_manager effort_controllers joint_state_controller kdl_parser - orocos_kdl - twil_gazebo_ros_control + geometry_msgs + nav_msgs + tf + control_toolbox + realtime_tools + message_runtime @@ -65,4 +73,4 @@ - \ No newline at end of file + diff --git a/twil_controllers/scripts/pose_step.py b/twil_controllers/scripts/pose_step.py new file mode 100755 index 0000000..f0c772e --- /dev/null +++ b/twil_controllers/scripts/pose_step.py @@ -0,0 +1,29 @@ +#!/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/scripts/pose_step.sh b/twil_controllers/scripts/pose_step.sh new file mode 100755 index 0000000..4d8e303 --- /dev/null +++ b/twil_controllers/scripts/pose_step.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +rostopic pub -1 /twil/nonsmooth_backstep_controller/command geometry_msgs/Pose2D "{x: $1, y: $2, theta: $3}" diff --git a/twil_controllers/src/nonsmooth_backstep_controller.cpp b/twil_controllers/src/nonsmooth_backstep_controller.cpp index a5efb91..5abdcd7 100644 --- a/twil_controllers/src/nonsmooth_backstep_controller.cpp +++ b/twil_controllers/src/nonsmooth_backstep_controller.cpp @@ -1,24 +1,29 @@ -#include -#include +#include +#include #include #include +#include + #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): - wheelRadius(2) + wheelRadius(2),lambda(5),gamma(4) { } NonSmoothBackstepController::~NonSmoothBackstepController(void) { sub_command_.shutdown(); + sub_parameters_.shutdown(); } bool NonSmoothBackstepController::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n) @@ -51,7 +56,51 @@ namespace twil_controllers hardware_interface::JointHandle j=robot->getHandle((std::string)name_value); joints_.push_back(j); } + + status_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"status",1)); + status_publisher_->msg_.header.frame_id="odom"; + + odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"odom",100)); + odom_publisher_->msg_.header.frame_id="odom"; + odom_publisher_->msg_.child_frame_id="twil"; + odom_publisher_->msg_.pose.pose.position.z=0; + odom_publisher_->msg_.pose.pose.orientation.x=0; + odom_publisher_->msg_.pose.pose.orientation.y=0; + + // Fake covariance + double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9}; + odom_publisher_->msg_.pose.covariance=boost::assign::list_of + (pose_cov[0]) (0) (0) (0) (0) (0) + (0) (pose_cov[1]) (0) (0) (0) (0) + (0) (0) (pose_cov[2]) (0) (0) (0) + (0) (0) (0) (pose_cov[3]) (0) (0) + (0) (0) (0) (0) (pose_cov[4]) (0) + (0) (0) (0) (0) (0) (pose_cov[5]); + + odom_publisher_->msg_.twist.twist.linear.z=0; + odom_publisher_->msg_.twist.twist.angular.x=0; + odom_publisher_->msg_.twist.twist.angular.y=0; + + //Fake covariance + double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9}; + odom_publisher_->msg_.twist.covariance=boost::assign::list_of + (twist_cov[0]) (0) (0) (0) (0) (0) + (0) (twist_cov[1]) (0) (0) (0) (0) + (0) (0) (twist_cov[2]) (0) (0) (0) + (0) (0) (0) (twist_cov[3]) (0) (0) + (0) (0) (0) (0) (twist_cov[4]) (0) + (0) (0) (0) (0) (0) (twist_cov[5]); + + tf_odom_publisher_.reset(new realtime_tools::RealtimePublisher(node_,"/tf",100)); + tf_odom_publisher_->msg_.transforms.resize(1); + tf_odom_publisher_->msg_.transforms[0].transform.translation.z=0.0; + tf_odom_publisher_->msg_.transforms[0].transform.rotation.x=0.0; + tf_odom_publisher_->msg_.transforms[0].transform.rotation.y=0.0; + tf_odom_publisher_->msg_.transforms[0].child_frame_id="twil_origin"; + tf_odom_publisher_->msg_.transforms[0].header.frame_id="odom"; + sub_command_=node_.subscribe("command",1000,&NonSmoothBackstepController::commandCB,this); + sub_parameters_=node_.subscribe("dynamic_parameters",1000,&NonSmoothBackstepController::parametersCB,this); std::string robot_desc_string; @@ -89,13 +138,27 @@ namespace twil_controllers segmentMapIter=tree.getSegment("right_wheel"); 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; + node_.param("gamma",gamma,gamma); + + lambda[0]=1; + lambda[1]=1; + lambda[2]=1; + lambda[3]=1; + lambda[4]=1; + node_.param("lambda",lambda,lambda); - 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; + const double K5=0.08444758509282763; + const double K6=3.770688129256381; + const double K7=2.6468901285322475; + const double K8=-16.084061415321404; + + Ginv << 0.5/K7, 0.5/K8, + 0.5/K7, -0.5/K8; F << 0.0, K5, K6, 0.0; @@ -104,137 +167,217 @@ namespace twil_controllers void NonSmoothBackstepController::starting(const ros::Time& time) { - xi.setZero(); - xiRef.setZero(); + x.setZero(); + xRef.setZero(); eta.setZero(); - + lastSamplingTime=time; } void NonSmoothBackstepController::update(const ros::Time& time, const ros::Duration& duration) { - if(duration.toSec() < 0.01) return; - - Eigen::Vector2d nu; + double dt=time.toSec()-lastSamplingTime.toSec(); + + if(fabs(dt-0.01) > 0.001/2) return; + lastSamplingTime=time; + + Eigen::Vector2d w; for(unsigned int i=0;i < joints_.size();i++) { - nu[i]=joints_[i].getVelocity(); + w[i]=joints_[i].getVelocity(); } - + + // w[0] is the left wheel velocity + // w[1] is the right wheel velocity 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; - + 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); - B << cos(xi[2]+u[1]*duration.toSec()/2.0), 0.0, - sin(xi[2]+u[1]*duration.toSec()/2.0), 0.0, + double deltaTheta2=u[1]*dt/2.0; + double deltaS=(fabs(deltaTheta2) > 1e-10)? sin(deltaTheta2)/deltaTheta2:1.0; + + B << cos(x[2]+deltaS*deltaTheta2), 0.0, + sin(x[2]+deltaS*deltaTheta2), 0.0, 0.0, 1.0; - - xi+=B*u*duration.toSec(); - + + Eigen::Vector3d dx=B*u; + + x+=dx*dt; + // Change of coordinates Eigen::Matrix3d R; - R << cos(xiRef[2]), sin(xiRef[2]), 0.0, - -sin(xiRef[2]), cos(xiRef[2]), 0.0, + R << cos(xRef[2]), sin(xRef[2]), 0.0, + -sin(xRef[2]), cos(xRef[2]), 0.0, 0.0, 0.0, 1.0; - Eigen::Vector3d xBar=R*(xi-xiRef); + Eigen::Vector3d xBar=R*(x-xRef); // 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); + eta[0]=-gamma[0]*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; + 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; 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 + if(fabs(e) > 1e-10) vBar[0]=-gamma[2]*eb[0]-lambda[0]/lambda[3]*cos(alpha) + +lambda[1]/lambda[3]*alpha*sin(alpha)/e + -lambda[2]/lambda[3]*psi*sin(alpha)/e; + 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(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(); + 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 - + Eigen::Vector2d v=vBar+deta; - // Linearization. Eigen::Vector2d uf; uf << u[0]*u[1], sqr(u[1]); - - Eigen::Vector2d torque=Ginv*(v+F*uf); + + 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]); } + + if(status_publisher_ && status_publisher_->trylock()) + { + status_publisher_->msg_.header.stamp=time; + + status_publisher_->msg_.set_point.x=xRef[0]; + status_publisher_->msg_.set_point.y=xRef[1]; + status_publisher_->msg_.set_point.theta=xRef[2]; + + 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_.error.x=xRef[0]-x[0]; + status_publisher_->msg_.error.y=xRef[1]-x[1]; + status_publisher_->msg_.error.theta=xRef[2]-x[2]; + + status_publisher_->msg_.time_step=dt; + + for(int i=0;i < torque.size();i++) + status_publisher_->msg_.command[i]=torque[i]; + + for(int i=0;i < lambda.size();i++) + status_publisher_->msg_.lambda[i]=lambda[i]; + + for(int i=0;i < gamma.size();i++) + status_publisher_->msg_.gamma[i]=gamma[i]; + + status_publisher_->msg_.polar_error.range=e; + status_publisher_->msg_.polar_error.angle=psi; + status_publisher_->msg_.polar_error.orientation=alpha; + + for(int i=0;i < eta.size();i++) + status_publisher_->msg_.backstep_set_point[i]=eta[i]; + + for(int i=0;i < deta.size();i++) + status_publisher_->msg_.backstep_set_point_dot[i]=deta[i]; + + for(int i=0;i < u.size();i++) + status_publisher_->msg_.backstep_process_value[i]=u[i]; + + for(int i=0;i < eb.size();i++) + status_publisher_->msg_.backstep_error[i]=eb[i]; + + for(int i=0;i < vBar.size();i++) + status_publisher_->msg_.backstep_command[i]=vBar[i]; + + for(int i=0;i < v.size();i++) + status_publisher_->msg_.linear_dynamics_command[i]=v[i]; + + status_publisher_->unlockAndPublish(); + } + + if(odom_publisher_ && odom_publisher_->trylock()) + { + odom_publisher_->msg_.header.stamp=time; + + odom_publisher_->msg_.pose.pose.position.x=x[0]; + odom_publisher_->msg_.pose.pose.position.y=x[1]; + 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_->unlockAndPublish(); + } + + if(tf_odom_publisher_ && tf_odom_publisher_->trylock()) + { + 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]; + odom_frame.transform.rotation.z=sin(x[2]/2); + odom_frame.transform.rotation.w=cos(x[2]/2); + + tf_odom_publisher_->unlockAndPublish(); + } } - void NonSmoothBackstepController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command) + void NonSmoothBackstepController::commandCB(const geometry_msgs::Pose2D::ConstPtr &command) { - for(unsigned int i=0;i < command->data.size() && i < 2;i++) xiRef[i]=command->data[i]; + xRef[0]=command->x; + xRef[1]=command->y; + xRef[2]=command->theta; } - void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command) + void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr ¶m) { // 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 + // Ginv=[0.5/K7 0.5/K8 + // 0.5/K7 -0.5/K8] - 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) + if(param->data[4] < 1e-3) F(0,1)=param->data[0]; + if(param->data[5] < 1e-3) F(1,0)=param->data[1]; + if(param->data[6] < 1e-3) { - Ginv(0,0)=0.5*command->data[2]; - Ginv(1,0)=0.5*command->data[2]; + Ginv(0,0)=0.5/param->data[2]; + Ginv(1,0)=0.5/param->data[2]; } - if(command->data[7] < 1e-3) + if(param->data[7] < 1e-3) { - Ginv(0,1)=0.5*command->data[3]; - Ginv(1,1)=-0.5*command->data[3]; + Ginv(0,1)=0.5/param->data[3]; + Ginv(1,1)=-0.5/param->data[3]; } } diff --git a/twil_description/doc/bouncing.txt b/twil_description/doc/bouncing.txt new file mode 100644 index 0000000..65b6eff --- /dev/null +++ b/twil_description/doc/bouncing.txt @@ -0,0 +1,47 @@ +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/launch/display.launch b/twil_description/launch/display.launch new file mode 100644 index 0000000..b17a754 --- /dev/null +++ b/twil_description/launch/display.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/twil_description/launch/gazebo.launch b/twil_description/launch/gazebo.launch new file mode 100644 index 0000000..c59641e --- /dev/null +++ b/twil_description/launch/gazebo.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/twil_description/launch/twil.launch b/twil_description/launch/twil.launch index bb29d83..7b4cbb3 100644 --- a/twil_description/launch/twil.launch +++ b/twil_description/launch/twil.launch @@ -1,4 +1,6 @@ - - + + + + diff --git a/twil_description/launch/twil_sim.launch b/twil_description/launch/twil_sim.launch deleted file mode 100644 index 12f93b7..0000000 --- a/twil_description/launch/twil_sim.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/twil_description/launch/twil_wam.launch b/twil_description/launch/twil_wam.launch deleted file mode 100644 index a86b321..0000000 --- a/twil_description/launch/twil_wam.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/twil_description/launch/twil_wam_sim.launch b/twil_description/launch/twil_wam_sim.launch deleted file mode 100644 index 0edb037..0000000 --- a/twil_description/launch/twil_wam_sim.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/twil_description/rviz/urdf.rviz b/twil_description/rviz/urdf.rviz new file mode 100644 index 0000000..c1db83d --- /dev/null +++ b/twil_description/rviz/urdf.rviz @@ -0,0 +1,244 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Odometry1 + - /Pose1 + Splitter Ratio: 0.5 + Tree Height: 559 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: true + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + battery: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + castor_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + castor_support: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + castor_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chassis_top: + Alpha: 1 + Show Axes: false + Show Trail: false + cpu: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_castor_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_castor_support: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_castor_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel_support: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + motor_driver: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + power_supply: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_support: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + stepper_driver: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + twil_origin: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: true + Keep: 42 + Length: 1 + Name: Odometry + Position Tolerance: 0.3 + Topic: /twil/nonsmooth_backstep_controller/odom + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 85; 0; 255 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /twil/command_stamped + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 7.48193 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.785398 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002bafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000036000002ba000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000036000002ba0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002c200fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002ba00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 60 + Y: 60 diff --git a/twil_description/xacro/castor_wheel.urdf.xacro b/twil_description/xacro/castor_wheel.urdf.xacro index 21172a2..e697875 100644 --- a/twil_description/xacro/castor_wheel.urdf.xacro +++ b/twil_description/xacro/castor_wheel.urdf.xacro @@ -36,6 +36,15 @@ Gazebo/Grey + diff --git a/twil_description/xacro/fixed_wheel.urdf.xacro b/twil_description/xacro/fixed_wheel.urdf.xacro index 862bdf1..ae3f7b3 100644 --- a/twil_description/xacro/fixed_wheel.urdf.xacro +++ b/twil_description/xacro/fixed_wheel.urdf.xacro @@ -2,6 +2,8 @@ + + @@ -32,6 +34,7 @@ + @@ -47,6 +50,15 @@ Gazebo/FlatBlack + diff --git a/twil_ident/CMakeLists.txt b/twil_ident/CMakeLists.txt index 0521d8a..55300bb 100644 --- a/twil_ident/CMakeLists.txt +++ b/twil_ident/CMakeLists.txt @@ -4,7 +4,7 @@ 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) +find_package(catkin REQUIRED COMPONENTS roscpp kdl_parser) find_package(cmake_modules REQUIRED) diff --git a/twil_ident/launch/gazebo.launch b/twil_ident/launch/gazebo.launch new file mode 100644 index 0000000..df6206f --- /dev/null +++ b/twil_ident/launch/gazebo.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/twil_ident/launch/ident.launch b/twil_ident/launch/ident.launch index 002625c..3e6e69a 100644 --- a/twil_ident/launch/ident.launch +++ b/twil_ident/launch/ident.launch @@ -1,17 +1,5 @@ - - - - + - - - - - - - - + diff --git a/twil_ident/package.xml b/twil_ident/package.xml index 56efcfa..ffefa1b 100644 --- a/twil_ident/package.xml +++ b/twil_ident/package.xml @@ -7,7 +7,7 @@ - Walter Fetter Lages + Walter Fetter Lages @@ -42,8 +42,9 @@ catkin eigen + kdl_parser eigen - + kdl_parser diff --git a/twil_ident/src/ident.cpp b/twil_ident/src/ident.cpp index 028497e..c1ffaf5 100644 --- a/twil_ident/src/ident.cpp +++ b/twil_ident/src/ident.cpp @@ -6,6 +6,9 @@ #include #include +#include + + class Prbs { int index; @@ -76,17 +79,54 @@ class Ident 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) + nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),prbs(nJoints),wheelRadius(nJoints) { node_=node; - jointStatesSubscriber=node_.subscribe("/joint_states",1000,&Ident::jointStatesCB,this); + 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); @@ -126,11 +166,15 @@ void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) 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++) - { - u[i]=jointStates->velocity[i]; // u(k+1) torque[i]=jointStates->effort[i]; // torque(k) - } y+=u; y/=dt.toSec(); @@ -149,6 +193,11 @@ void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) 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]); diff --git a/twil_trajectories/CMakeLists.txt b/twil_trajectories/CMakeLists.txt new file mode 100644 index 0000000..a38b1ef --- /dev/null +++ b/twil_trajectories/CMakeLists.txt @@ -0,0 +1,177 @@ +cmake_minimum_required(VERSION 2.8.3) +project(twil_trajectories) + +## 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) + +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 ${PROJECT_NAME} +# 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( + include ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} +) + +## Declare a cpp library +add_library(${PROJECT_NAME} + src/circle_path.cpp + src/eight_path.cpp +) + +## Declare a cpp executable +add_executable(eight_trajectory src/eight_trajectory.cpp) +add_executable(pose2d_stamp src/pose2d_stamp.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(twil_trajectories_node twil_trajectories_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${eigen_LIBRARIES} +) + +target_link_libraries(eight_trajectory + ${catkin_LIBRARIES} + ${eigen_LIBRARIES} + ${PROJECT_NAME} +) + +target_link_libraries(pose2d_stamp + ${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_trajectories.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_trajectories/include/twil_trajectories/circle_path.h b/twil_trajectories/include/twil_trajectories/circle_path.h new file mode 100644 index 0000000..4c16412 --- /dev/null +++ b/twil_trajectories/include/twil_trajectories/circle_path.h @@ -0,0 +1,56 @@ +#ifndef CIRCLE_PATH_H +#define CIRCLE_PATH_H + +#include + +/** Circle path +* @author Walter Fetter Lages +*/ +class CirclePath +{ + Eigen::Vector2d pc_; + double phi0_; + double r_; + double w_; + + public: + + /** Build a circle path. + * @param pc circle center point. + * @param phi0 initial phase. + * @param r circle radius. + * @param w angular velocity. + */ + CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w); + + /** Build a circle path. + * @param p0 starting pose. + * @param r circle radius. + * @param w angular velocity. + */ + CirclePath(const Eigen::Vector3d &p0,double r,double w); + + /** Build a circle path. + * @param pc circle center point. + * @param p0 starting point. + * @param w angular velocity. + */ + CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w); + + /** Destroy a circle path. + */ + ~CirclePath(void) { }; + + /** Get path point. + * @param t path time. + * @return path point. + */ + Eigen::Vector3d point(double t) const; + + /** Get path steering controls. + * @param t path time. + * @return steering controls. + */ + Eigen::Vector2d steering(double t) const; +}; +#endif diff --git a/twil_trajectories/include/twil_trajectories/eight_path.h b/twil_trajectories/include/twil_trajectories/eight_path.h new file mode 100644 index 0000000..b3f5af2 --- /dev/null +++ b/twil_trajectories/include/twil_trajectories/eight_path.h @@ -0,0 +1,40 @@ +#ifndef EIGHT_PATH_H +#define EIGHT_PATH_H + +#include + +/** 8 path +* @author Walter Fetter Lages +*/ +class EightPath +{ + CirclePath c1_; + CirclePath c2_; + double period_; + + public: + + /** Build an 8 path. + * @param pc center point. + * @param r radius. + * @param w angular velocity. + */ + EightPath(const Eigen::Vector2d &pc,double r,double w); + + /** Destroy an 8 path. + */ + ~EightPath(void) { }; + + /** Get path point. + * @param t path time. + * @return path point. + */ + Eigen::Vector3d point(double t) const; + + /** Get path steering controls. + * @param t path time. + * @return steering controls. + */ + Eigen::Vector2d steering(double t) const; +}; +#endif diff --git a/twil_trajectories/package.xml b/twil_trajectories/package.xml new file mode 100644 index 0000000..8545852 --- /dev/null +++ b/twil_trajectories/package.xml @@ -0,0 +1,58 @@ + + + twil_trajectories + 2.0.0 + The twil_trajectories package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + eigen + geometry_msgs + + eigen + geometry_msgs + + + + + + + + + + diff --git a/twil_trajectories/src/circle_path.cpp b/twil_trajectories/src/circle_path.cpp new file mode 100644 index 0000000..8c7c805 --- /dev/null +++ b/twil_trajectories/src/circle_path.cpp @@ -0,0 +1,51 @@ +#include + +#define sqr(x) (x*x) +#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) + +CirclePath::CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w) +{ + pc_=pc; + phi0_=phi0; + r_=r; + w_=w; +} + +CirclePath::CirclePath(const Eigen::Vector3d &p0,double r,double w) +{ + phi0_=p0[2]-sgn(w)*M_PI_2; + pc_[0]=p0[0]-r*cos(phi0_); + pc_[1]=p0[1]-r*sin(phi0_); + r_=r; + w_=w; +} + +CirclePath::CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w) +{ + pc_=pc; + w_=w; + phi0_=atan2(p0[1]-pc[1],p0[0]-pc[0]); + r_=sqrt(sqr(p0[1]-pc[1])+sqr(p0[0]-pc[0])); +} + +Eigen::Vector3d CirclePath::point(double t) const +{ + double wt=w_*t; + Eigen::Vector3d p; + + p[0]=pc_[0]+r_*cos(wt+phi0_); + p[1]=pc_[1]+r_*sin(wt+phi0_); + p[2]=wt+phi0_+sgn(w_)*M_PI_2; + + return p; +} + +Eigen::Vector2d CirclePath::steering(double t) const +{ + Eigen::Vector2d e(2); + + e[0]=w_*r_; + e[1]=w_; + + return e; +} diff --git a/twil_trajectories/src/eight_path.cpp b/twil_trajectories/src/eight_path.cpp new file mode 100644 index 0000000..3a7603f --- /dev/null +++ b/twil_trajectories/src/eight_path.cpp @@ -0,0 +1,26 @@ +#include + +#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) + +EightPath::EightPath(const Eigen::Vector2d &pc,double r,double w): +c1_(Eigen::Vector2d(pc[0],pc[1]+r),-M_PI_2*sgn(w),r,w), +c2_(Eigen::Vector2d(pc[0],pc[1]-r),2.5*M_PI*sgn(w),r,-w) +{ + period_=4*M_PI/w; +}; + +Eigen::Vector3d EightPath::point(double t) const +{ + double tc=fmod(t,period_/2.0); + + if(int(t/(period_/2.0)) % 2 == 0) return c1_.point(tc); + else return c2_.point(tc); +} + +Eigen::Vector2d EightPath::steering(double t) const +{ + double tc=fmod(t,period_/2.0); + + if(int(t/(period_/2.0)) % 2 == 0) return c1_.steering(tc); + else return c2_.steering(tc); +} diff --git a/twil_trajectories/src/eight_trajectory.cpp b/twil_trajectories/src/eight_trajectory.cpp new file mode 100644 index 0000000..6f0a059 --- /dev/null +++ b/twil_trajectories/src/eight_trajectory.cpp @@ -0,0 +1,73 @@ +#include +#include + +#include + +class EightTrajectory +{ + public: + EightTrajectory(ros::NodeHandle node); + ~EightTrajectory(void); + void setCommand(ros::Duration t); + + private: + ros::NodeHandle node_; + const EightPath *path; + + ros::Publisher commandPublisher; +}; + + +EightTrajectory::EightTrajectory(ros::NodeHandle node) +{ + node_=node; + Eigen::Vector2d pc; + double w; + double r; + + ros::param::get("~x",pc[0]); + ros::param::get("~y",pc[1]); + ros::param::get("~radius",r); + ros::param::get("~ang_vel",w); + + path=new EightPath(pc,r,w); + commandPublisher=node_.advertise("command",1000); +} + +EightTrajectory::~EightTrajectory(void) +{ + commandPublisher.shutdown(); + delete path; +} + +void EightTrajectory::setCommand(ros::Duration t) +{ + Eigen::Vector3d p=path->point(t.toSec()); + + geometry_msgs::Pose2D command; + command.x=p[0]; + command.y=p[1]; + command.theta=p[2]; + commandPublisher.publish(command); +} + +int main(int argc,char* argv[]) +{ + + ros::init(argc,argv,"eight_trajectory"); + ros::NodeHandle node; + + EightTrajectory eightTrajectory(node); + + ros::Rate loop(100); + + ros::Time t0=ros::Time::now(); + while(ros::ok()) + { + eightTrajectory.setCommand(ros::Time::now()-t0); + + ros::spinOnce(); + loop.sleep(); + } + return 0; +} diff --git a/twil_trajectories/src/pose2d_stamp.cpp b/twil_trajectories/src/pose2d_stamp.cpp new file mode 100644 index 0000000..84e0164 --- /dev/null +++ b/twil_trajectories/src/pose2d_stamp.cpp @@ -0,0 +1,75 @@ +#include + +#include + +#include +#include + +class Pose2DStamp +{ + public: + Pose2DStamp(ros::NodeHandle node); + ~Pose2DStamp(void); + + private: + ros::NodeHandle node_; + + ros::Subscriber poseSubscriber; + ros::Publisher posePublisher; + + int seq; + std::string frame_id; + + void poseCB(const geometry_msgs::Pose2D::ConstPtr &pose); +}; + + +Pose2DStamp::Pose2DStamp(ros::NodeHandle node) +{ + node_=node; + + poseSubscriber=node_.subscribe("command",1000,&Pose2DStamp::poseCB,this); + posePublisher=node_.advertise("command_stamped",1000); + + seq=0; + + ros::param::get("~frame_id",frame_id); +} + +Pose2DStamp::~Pose2DStamp(void) +{ + poseSubscriber.shutdown(); + posePublisher.shutdown(); +} + +void Pose2DStamp::poseCB(const geometry_msgs::Pose2D::ConstPtr &pose) +{ + geometry_msgs::PoseStamped stamped; + stamped.header.stamp=ros::Time::now(); + stamped.header.frame_id=frame_id; + stamped.pose.position.x=pose->x; + stamped.pose.position.y=pose->y; + stamped.pose.position.z=0; + stamped.pose.orientation.x=0; + stamped.pose.orientation.y=0; + stamped.pose.orientation.z=sin(pose->theta/2.0); + stamped.pose.orientation.w=cos(pose->theta/2.0); + posePublisher.publish(stamped); +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"pose2d_stamp"); + ros::NodeHandle node; + + Pose2DStamp pose2DStamp(node); + + ros::Rate loop(100); + ros::Time t0=ros::Time::now(); + while(ros::ok()) + { + ros::spinOnce(); + loop.sleep(); + } + return 0; +}