+++ /dev/null
-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))}'
-# Watch-out: The indenting here is relevant to the semantic!
+# Watch-out: The indentation here is relevant to the semantic!
twil:
+++ /dev/null
-#ifndef TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_H
-#define TWIL_CONTROLLERS_CART_LINEARIZING_CONTROLLER_RNE_H
-
-#include <cstddef>
-#include <vector>
-#include <string>
-
-#include <ros/node_handle.h>
-#include <hardware_interface/joint_command_interface.h>
-#include <controller_interface/controller.h>
-#include <std_msgs/Float64MultiArray.h>
-#include <sensor_msgs/JointState.h>
-
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-#include <kdl/chainidsolver_recursive_newton_euler.hpp>
-
-
-namespace twil_controllers
-{
- class CartLinearizingController_RNE: public controller_interface::Controller<hardware_interface::EffortJointInterface>
- {
- 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<hardware_interface::JointHandle> 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<double> v;
-
- KDL::Wrenches fext;
-
- std::vector<double> wheelRadius;
- double wheelBase;
- };
-}
-#endif
namespace twil_controllers
{
- class NonSmoothBackstepController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+ class NonSmoothBackstepController: public controller_interface::
+ Controller<hardware_interface::EffortJointInterface>
{
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<hardware_interface::JointHandle> joints_;
+ ros::NodeHandle node_;
+ hardware_interface::EffortJointInterface *robot_;
+ std::vector<hardware_interface::JointHandle> joints_;
- boost::scoped_ptr<realtime_tools::RealtimePublisher
- <twil_controllers::NonSmoothBackstepControllerStatus>
- > status_publisher_ ;
+ boost::scoped_ptr<realtime_tools::RealtimePublisher
+ <twil_controllers::NonSmoothBackstepControllerStatus>
+ > status_publisher_ ;
- boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_publisher_;
- boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_publisher_;
+ boost::shared_ptr<realtime_tools::RealtimePublisher
+ <nav_msgs::Odometry> > odom_publisher_;
+ boost::shared_ptr<realtime_tools::RealtimePublisher
+ <tf::tfMessage> > 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<double> wheelRadius;
- double wheelBase;
+ std::vector<double> 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<double> lambda;
- std::vector<double> gamma;
+ Eigen::Vector2d phi;
- void commandCB(const geometry_msgs::Pose2D::ConstPtr &command);
- void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command);
+ std::vector<double> lambda;
+ std::vector<double> gamma;
+
+ void commandCB(const geometry_msgs::Pose2D::ConstPtr &command);
+ void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command);
};
}
#endif
+++ /dev/null
-<launch>
- <remap from="/twil/nonsmooth_backstep_controller/dynamic_parameters" to="/twil/dynamic_parameters"/>
-
- <rosparam file="$(find twil_controllers)/config/nonsmooth_backstep_control.yaml" command="load"/>
-
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="twil" args="joint_state_controller nonsmooth_backstep_controller"/>
-
- <node name="ident" ns="twil" pkg="twil_ident" type="ident" output="screen">
- <remap from="ident/dynamic_parameters" to="dynamic_parameters"/>
- </node>
-
-</launch>
<arg name="use_sim_time" default="true"/>
<arg name="wam" default="false"/>
- <remap from="/twil/nonsmooth_backstep_controller/command" to="/twil/command" />
- <remap from="/twil/joint_states" to="/joint_states" />
+ <remap from="/twil/joint_states" to="/joint_states" />
<include file="$(find twil_description)/launch/gazebo.launch" >
<arg name="paused" value="$(arg paused)"/>
<arg name="wam" value="$(arg wam)"/>
</include>
- <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
-
- <node name="eight_trajectory" pkg="twil_trajectories" type="eight_trajectory" respawn="false" output="screen" ns="twil" args="_x:=0 _y:=0 _radius:=1.0 _ang_vel:=0.2" />
-
- [B<node name="pose2d_stamp" pkg="twil_trajectories" type="pose2d_stamp" respawn="false" output="screen" ns="twil" args="_frame_id:=map" />
-
- <node pkg="tf2_ros" type="static_transform_publisher" name="odom_publisher" args="0 0 0 0 0 0 1 map odom" />
-
- <include file="$(find twil_description)/launch/display.launch" />
+ <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
</launch>
--- /dev/null
+<launch>
+ <arg name="paused" default="true"/>
+ <arg name="headless" default="false"/>
+ <arg name="use_sim_time" default="true"/>
+ <arg name="wam" default="false"/>
+
+ <remap from="/twil/command" to="/twil/nonsmooth_backstep_controller/command" />
+ <remap from="/twil/joint_states" to="/joint_states" />
+
+ <include file="$(find twil_description)/launch/gazebo.launch" >
+ <arg name="paused" value="$(arg paused)"/>
+ <arg name="headless" value="$(arg headless)"/>
+ <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+ <arg name="wam" value="$(arg wam)"/>
+ </include>
+
+ <include file="$(find twil_controllers)/launch/nonsmooth_backstep.launch" />
+
+ <node name="eight_trajectory" pkg="twil_trajectories" type="eight_trajectory" respawn="false" output="screen" ns="twil" args="_x:=0.0 _y:=-0.5 _radius:=1.0 _ang_vel:=0.1"/>
+
+ <node name="pose2d_stamp" pkg="twil_trajectories" type="pose2d_stamp" respawn="false" output="screen" ns="twil" args="_frame_id:=map" />
+
+ <node pkg="tf2_ros" type="static_transform_publisher" name="odom_publisher" args="0 0 0 0 0 0 1 map odom" />
+
+ <include file="$(find twil_description)/launch/display.launch" />
+
+</launch>
+++ /dev/null
-#!/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
+++ /dev/null
-#include <twil_controllers/cart_linearizing_controller_rne.h>
-#include <pluginlib/class_list_macros.h>
-
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-#include <kdl/chainidsolver_recursive_newton_euler.hpp>
-
-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)
#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):
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;
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,
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,
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;
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]);
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];
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];
+++ /dev/null
-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:
-
-<gazebo reference="my_link">
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <mu1>0.8</mu1>
- <mu2>0.8</mu2>
- <maxVel>0.0</maxVel>
- <minDepth>0.001</minDepth>
-</gazebo>
-
+++ /dev/null
-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)
+++ /dev/null
-<?xml version="1.0"?>
-<package>
- <name>twil_gazebo_ros_control</name>
- <version>2.0.0</version>
- <description>The twil_gazebo_ros_control package</description>
-
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
- <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
- <license>GPLv3</license>
-
-
- <!-- Url tags are optional, but mutiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/twil_gazebo_ros_control</url> -->
-
-
- <!-- Author tags are optional, mutiple are allowed, one per tag -->
- <!-- Authors do not have to be maintianers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
- <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
-
-
- <!-- The *_depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use run_depend for packages you need at runtime: -->
- <!-- <run_depend>message_runtime</run_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <buildtool_depend>catkin</buildtool_depend>
-
- <build_depend>gazebo</build_depend>
- <build_depend>gazebo_ros_control</build_depend>
- <build_depend>controller_manager</build_depend>
- <build_depend>pluginlib</build_depend>
- <build_depend>twil_description</build_depend>
-
- <run_depend>controller_manager</run_depend>
- <run_depend>pluginlib</run_depend>
- <!--run_depend>gazebo</run_depend-->
- <run_depend>gazebo_ros_control</run_depend>
- <run_depend>twil_description</run_depend>
-
-
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <!-- You can specify that this package is a metapackage here: -->
- <!-- <metapackage/> -->
-
- <!-- Other tools can request additional information be placed here -->
-
- </export>
-</package>
\ No newline at end of file
+++ /dev/null
-#include <pluginlib/class_list_macros.h>
-
-#include <hardware_interface/joint_command_interface.h>
-#include <hardware_interface/robot_hw.h>
-
-#include <gazebo_ros_control/robot_hw_sim.h>
-
-#include <angles/angles.h>
-
-#include <gazebo/gazebo.hh>
-#include <gazebo/physics/physics.hh>
-#include <gazebo/common/common.hh>
-
-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<std::string> joint_name_;
- std::vector<double> joint_position_;
- std::vector<double> joint_velocity_;
- std::vector<double> joint_effort_;
- std::vector<double> joint_effort_command_;
-
- std::vector<gazebo::physics::JointPtr> 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<transmission_interface::TransmissionInfo> 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)
+++ /dev/null
-<library path="lib/libtwil_gazebo_ros_control">
-
- <class
- name="twil_gazebo_ros_control/TwilRobotHWSim"
- type="twil_gazebo_ros_control::TwilRobotHWSim"
- base_class_type="gazebo_ros_control::RobotHWSim">
- <description>
- A ROS/Gazebo interface Twil, exporting a joint_state_interface and a
- joint_effort_interface.
- </description>
- </class>
-</library>
+++ /dev/null
-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)
+++ /dev/null
-<launch>
- <arg name="paused" default="true"/>
- <arg name="headless" default="false"/>
- <arg name="use_sim_time" default="true"/>
-
- <remap from="/twil/left_wheel_joint_effort_controller/command" to="/twil/left_wheel_command"/>
- <remap from="/twil/right_wheel_joint_effort_controller/command" to="/twil/right_wheel_command"/>
-
- <remap from="/twil/ident/left_wheel_command" to="/twil/left_wheel_command"/>
- <remap from="/twil/ident/right_wheel_command" to="/twil/right_wheel_command"/>
-
- <include file="$(find twil_description)/launch/gazebo.launch" >
- <arg name="paused" value="$(arg paused)"/>
- <arg name="headless" value="$(arg headless)"/>
- <arg name="use_sim_time" value="$(arg use_sim_time)"/>
- </include>
-
- <include file="$(find twil_ident)/launch/ident.launch"/>
-</launch>
+++ /dev/null
-<launch>
- <include file="$(find twil_controllers)/launch/joint_effort.launch" />
-
- <node name="ident" ns="twil" pkg="twil_ident" type="ident" output="screen" />
-</launch>
+++ /dev/null
-<?xml version="1.0"?>
-<package>
- <name>twil_ident</name>
- <version>2.0.0</version>
- <description>The twil_ident package</description>
-
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
- <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
- <license>GPLv3</license>
-
-
- <!-- Url tags are optional, but mutiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/twil_ident</url> -->
-
-
- <!-- Author tags are optional, mutiple are allowed, one per tag -->
- <!-- Authors do not have to be maintianers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
- <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
-
-
- <!-- The *_depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use run_depend for packages you need at runtime: -->
- <!-- <run_depend>message_runtime</run_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>eigen</build_depend>
- <build_depend>kdl_parser</build_depend>
- <run_depend>eigen</run_depend>
- <run_depend>kdl_parser</run_depend>
-
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <!-- You can specify that this package is a metapackage here: -->
- <!-- <metapackage/> -->
-
- <!-- Other tools can request additional information be placed here -->
-
- </export>
-</package>
+++ /dev/null
-#include <ros/ros.h>
-
-#include <Eigen/Dense>
-
-#include <std_msgs/Float64.h>
-#include <std_msgs/Float64MultiArray.h>
-#include <sensor_msgs/JointState.h>
-
-#include <kdl_parser/kdl_parser.hpp>
-
-
-class Prbs
-{
- int index;
- unsigned int nd;
- unsigned char *sh;
-
- public:
-
- Prbs(unsigned int n=10,unsigned int seed=0);
- ~Prbs(void);
-
- void seed(unsigned int s) { index=s % nd; };
-
- operator int(void);
-};
-
-
-Prbs::Prbs(unsigned int n,unsigned int seed)
-{
- nd=n;
- index=seed % nd;
- sh=new unsigned char[nd];
- for(unsigned int i=0; i < nd;i++) sh[i]=1;
- for(unsigned int i=0; i < 98;i++) (int) *this; // call operator int() to exercise
-
-}
-
-Prbs::~Prbs(void)
-{
-// delete[] sh;
-}
-
-Prbs::operator int(void)
-{
- unsigned char s=sh[nd-1]+sh[nd-2];
- if(s > 1) s=0;
- for(int j=nd-2;j >= 0;j--) sh[j+1]=sh[j];
- sh[0]=s;
- return sh[index];
-}
-
-
-class Ident
-{
- public:
- Ident(ros::NodeHandle node);
- ~Ident(void);
- void setCommand(void);
-
- private:
- ros::NodeHandle node_;
-
- ros::Subscriber jointStatesSubscriber;
- ros::Publisher dynParamPublisher;
- ros::Publisher leftWheelCommandPublisher;
- ros::Publisher rightWheelCommandPublisher;
-
- const int nJoints;
-
- Eigen::VectorXd u;
- Eigen::VectorXd thetaEst1;
- Eigen::VectorXd thetaEst2;
- Eigen::MatrixXd P1;
- Eigen::MatrixXd P2;
-
- std::vector<Prbs> prbs;
- int iter;
-
- ros::Time lastTime;
-
- std::vector<double> 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<std_msgs::Float64MultiArray>("ident/dynamic_parameters",1000);
- leftWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/left_wheel_command",1000);
- rightWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/right_wheel_command",1000);
-
- u.setZero();
- thetaEst1.setZero();
- thetaEst2.setZero();
- resetCovariance();
-
- lastTime=ros::Time::now();
-}
-
-Ident::~Ident(void)
-{
- jointStatesSubscriber.shutdown();
-}
-
-void Ident::resetCovariance(void)
-{
- P1.setIdentity();
- P1*=1;
- P2.setIdentity();
- P2*=1;
- iter=0;
-}
-
-void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates)
-{
- ros::Duration dt=jointStates->header.stamp-lastTime;
- lastTime=jointStates->header.stamp;
-
- Eigen::VectorXd y=-u; //y(k+1)=(u(k+1)-u(k))/dt
-
- Eigen::VectorXd Phi1(nJoints);
- Eigen::VectorXd Phi2(nJoints);
- Phi1[0]=u[1]*u[1]; // u2^2(k)
- Phi2[0]=u[0]*u[1]; // u1(k)*u2(k)
-
- Eigen::VectorXd torque(nJoints);
-
- // 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;
-}
projects / twil.git / commitdiff