## Declare a cpp library
add_library(twil_controllers
src/cart_linearizing_controller.cpp
- src/cart_linearizing_controller_rne.cpp
- src/nonsmooth_backstep_controller.cpp
)
## Declare a cpp executable
+++ /dev/null
-twil:
-
- joint_state_controller:
- type: joint_state_controller/JointStateController
- publish_rate: 100
-
- nonsmooth_backstep_controller:
- type: twil_controllers/NonSmoothBackstepController
- joints:
- - left_wheel_joint
- - right_wheel_joint
+++ /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
+++ /dev/null
-#ifndef TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_H
-#define TWIL_CONTROLLERS_NONSMOOTH_BACKSTEP_CONTROLLER_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 <Eigen/Dense>
-
-namespace twil_controllers
-{
- class NonSmoothBackstepController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
- {
- public:
- NonSmoothBackstepController(void);
- ~NonSmoothBackstepController(void);
-
- bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
- void starting(const ros::Time& time);
- void update(const ros::Time& time,const ros::Duration& duration);
-
- private:
- ros::NodeHandle node_;
- hardware_interface::EffortJointInterface *robot_;
- std::vector<hardware_interface::JointHandle> joints_;
-
- ros::Subscriber sub_command_;
- ros::Subscriber sub_parameters_;
-
- Eigen::Matrix2d Ginv;
- Eigen::Matrix2d F;
-
- std::vector<double> wheelRadius;
- double wheelBase;
-
- Eigen::Vector3d xi;
- Eigen::Vector3d xiRef;
-
- Eigen::Vector2d eta;
-
- void commandCB(const std_msgs::Float64MultiArray::ConstPtr &command);
- void parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command);
- };
-}
-#endif
+++ /dev/null
-<launch>
- <remap from="/twil/nonsmooth_backstep_controller/dynamic_parameters" to="/twil/dynamic_parameters"/>
-
- <arg name="paused" default="true"/>
-
- <include file="$(find twil_description)/launch/twil_sim.launch">
- <arg name="paused" value="$(arg paused)"/>
- </include>
-
- <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>
-
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
-
-</launch>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/twil" args="joint_state_controller cart_linearizing_controller"/>
- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" ns="/twil" />
</launch>
+++ /dev/null
-<launch>
-
- <arg name="paused" default="true"/>
-
- <include file="$(find twil_description)/launch/twil_sim.launch">
- <arg name="paused" value="$(arg paused)"/>
- </include>
-
- <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="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
-
-</launch>
+++ /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)
+++ /dev/null
-#include <twil_controllers/nonsmooth_backstep_controller.h>
-#include <pluginlib/class_list_macros.h>
-
-#include <kdl/frames.hpp>
-#include <kdl_parser/kdl_parser.hpp>
-
-#define sqr(x) (x*x)
-#define cub(x) (x*x*x)
-
-#define NUMERICAL_DETA
-
-namespace twil_controllers
-{
- NonSmoothBackstepController::NonSmoothBackstepController(void):
- wheelRadius(2)
- {
- }
-
- NonSmoothBackstepController::~NonSmoothBackstepController(void)
- {
- sub_command_.shutdown();
- }
-
- bool NonSmoothBackstepController::init(hardware_interface::EffortJointInterface *robot,ros::NodeHandle &n)
- {
- node_=n;
- robot_=robot;
-
- XmlRpc::XmlRpcValue joint_names;
- if(!node_.getParam("joints",joint_names))
- {
- ROS_ERROR("No 'joints' parameter in controller. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- if(joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
- {
- ROS_ERROR("The 'joints' parameter is not a struct. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- for(int i=0; i < joint_names.size();i++)
- {
- XmlRpc::XmlRpcValue &name_value=joint_names[i];
- if(name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
- {
- ROS_ERROR("Array of joint names should contain only strings. (namespace: %s)",node_.getNamespace().c_str());
- return false;
- }
-
- hardware_interface::JointHandle j=robot->getHandle((std::string)name_value);
- joints_.push_back(j);
- }
- sub_command_=node_.subscribe("command",1000,&NonSmoothBackstepController::commandCB,this);
- sub_parameters_=node_.subscribe("dynamic_parameters",1000,&NonSmoothBackstepController::parametersCB,this);
-
- std::string robot_desc_string;
- if(!node_.getParam("/robot_description",robot_desc_string))
- {
- ROS_ERROR("Could not find '/robot_description'.");
- return false;
- }
-
- KDL::Tree tree;
- if (!kdl_parser::treeFromString(robot_desc_string,tree))
- {
- ROS_ERROR("Failed to construct KDL tree.");
- return false;
- }
-
- // get wheelBase from URDF (actually from KDL tree)
- KDL::SegmentMap::const_iterator segmentMapIter=tree.getSegment("left_wheel_support");
- KDL::Frame leftSupportFrame=segmentMapIter->second.segment.getFrameToTip();
-
- segmentMapIter=tree.getSegment("right_wheel_support");
- KDL::Frame rightSupportFrame=segmentMapIter->second.segment.getFrameToTip();
-
- wheelRadius.resize(joints_.size());
- wheelBase=leftSupportFrame(1,3)-rightSupportFrame(1,3);
-
- // get wheelRadius from URDF (actually from KDL tree)
- segmentMapIter=tree.getSegment("chassis");
- KDL::Frame chassisFrame=segmentMapIter->second.segment.getFrameToTip();
-
- segmentMapIter=tree.getSegment("left_wheel");
- KDL::Joint leftWheelJoint=segmentMapIter->second.segment.getJoint();
- wheelRadius[0]=chassisFrame(2,3)+leftSupportFrame(2,3)+leftWheelJoint.JointOrigin().z();
-
- segmentMapIter=tree.getSegment("right_wheel");
- KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint();
- wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z();
-
- const double K5 = 0.0018533548425194695;
- const double K6 = 0.09946140462774823;
- const double K7 = 21.65458426501294;
- const double K8 = -15.40102896939387;
- Ginv << 0.5*K7, 0.5*K8,
- 0.5*K7, -0.5*K8;
- F << 0.0, K5,
- K6, 0.0;
-
- return true;
- }
-
- void NonSmoothBackstepController::starting(const ros::Time& time)
- {
- xi.setZero();
- xiRef.setZero();
- eta.setZero();
-
- }
-
- void NonSmoothBackstepController::update(const ros::Time& time,
- const ros::Duration& duration)
- {
- if(duration.toSec() < 0.01) return;
-
- Eigen::Vector2d nu;
- for(unsigned int i=0;i < joints_.size();i++)
- {
- nu[i]=joints_[i].getVelocity();
- }
-
- Eigen::Vector2d u;
- u[0]=(nu[0]*wheelRadius[0]+nu[1]*wheelRadius[1])/2.0;
- u[1]=(nu[1]*wheelRadius[1]-nu[0]*wheelRadius[0])/wheelBase;
-
- // Estimate pose by odometry
- Eigen::MatrixXd B(3,2);
- B << cos(xi[2]+u[1]*duration.toSec()/2.0), 0.0,
- sin(xi[2]+u[1]*duration.toSec()/2.0), 0.0,
- 0.0, 1.0;
-
- xi+=B*u*duration.toSec();
-
- // Change of coordinates
- Eigen::Matrix3d R;
- R << cos(xiRef[2]), sin(xiRef[2]), 0.0,
- -sin(xiRef[2]), cos(xiRef[2]), 0.0,
- 0.0, 0.0, 1.0;
- Eigen::Vector3d xBar=R*(xi-xiRef);
-
- // Discontinuous transformation
- double e=sqrt(sqr(xBar[0])+sqr(xBar[1]));
- double psi=atan2(xBar[1],xBar[0]);
- double alpha=xBar[2]-psi;
-
- // Backstep controller
- const double GAMMA1=10.0;
- const double GAMMA2=1.0;
-// const double GAMMA3=1.0;
- const double GAMMA4=10.0;
- const double GAMMA5=1.0;
- const double LAMBDA1=1.0;
- const double LAMBDA2=0.1;
- const double LAMBDA3=0.1;
- const double LAMBDA4=10.0;
- const double LAMBDA5=100.0;
-
-#ifdef NUMERICAL_DETA
- Eigen::Vector2d deta=-eta;
-#endif
- Eigen::Vector2d eta;
-
- eta[0]=-GAMMA1*e*cos(alpha);
-
- if(alpha > 1e-6) eta[1]=-GAMMA2*alpha-GAMMA1*sin(alpha)*cos(alpha)+GAMMA1*LAMBDA3*psi*sin(alpha)/LAMBDA2/alpha*cos(alpha);
- else eta[1]=GAMMA1*LAMBDA3*psi/LAMBDA2;
-
- Eigen::Vector2d eb=u-eta;
-
- Eigen::Vector2d vBar;
- if(e > 1e-6) vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha)
- +LAMBDA2/LAMBDA4*alpha*sin(alpha)/e
- -LAMBDA3/LAMBDA4*psi*sin(alpha)/e;
- else vBar[0]=-GAMMA4*eb[0]-LAMBDA1/LAMBDA4*cos(alpha);
- vBar[1]=-GAMMA5*eb[1]-LAMBDA2/LAMBDA5*alpha;
-
-#ifndef NUMERICAL_DETA
- // Analitic derivates
- Eigen::Vector2d deta;
- deta[0]=sqr(GAMMA1)*e*cub(cos(alpha))-GAMMA1*GAMMA2*e*alpha*sin(alpha)
- +sqr(GAMMA1)*LAMBDA3/LAMBDA2*e*cos(alpha)*sqr(sin(alpha))/alpha*psi;
- deta[1]=sqr(GAMMA2)*alpha-2.0*GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi*sin(alpha)/alpha*cos(alpha)
- +GAMMA1*GAMMA2*alpha*sqr(cos(alpha))-sqr(GAMMA1)*LAMBDA3/LAMBDA2*cub(cos(alpha))*sin(alpha)/alpha*psi
- -GAMMA1*GAMMA2*alpha*sqr(sin(alpha))+sqr(GAMMA1)*LAMBDA3/LAMBDA2*cos(alpha)*cub(sin(alpha))/alpha*psi
- -sqr(GAMMA1)*LAMBDA3/LAMBDA2*sqr(cos(alpha))*sqr(sin(alpha))/alpha-GAMMA1*GAMMA2*LAMBDA3/LAMBDA2*psi
- +sqr(GAMMA1*LAMBDA3/LAMBDA2)*cub(cos(alpha))*sin(alpha)/sqr(alpha)*sqr(psi)+sqr(GAMMA1*LAMBDA3/LAMBDA2)*cos(alpha)*cub(sin(alpha))/sqr(alpha)*sqr(psi)
- +sqr(GAMMA1*LAMBDA3/LAMBDA2*cos(alpha)*sin(alpha)*psi)/cub(alpha);
-#else
- deta+=eta;
- deta/=duration.toSec();
-#endif
-
- Eigen::Vector2d v=vBar+deta;
-
-
- // Linearization.
- Eigen::Vector2d uf;
- uf << u[0]*u[1], sqr(u[1]);
-
- Eigen::Vector2d torque=Ginv*(v+F*uf);
-
- // Apply torques
- for(unsigned int i=0;i < joints_.size();i++)
- {
- if(torque[i] > 100.0) torque[i]=100.0;
- if(torque[i] < -100.0) torque[i]=-100.0;
- joints_[i].setCommand(torque[i]);
- }
- }
-
- void NonSmoothBackstepController::commandCB(const std_msgs::Float64MultiArray::ConstPtr &command)
- {
- for(unsigned int i=0;i < command->data.size() && i < 2;i++) xiRef[i]=command->data[i];
- }
-
- void NonSmoothBackstepController::parametersCB(const std_msgs::Float64MultiArray::ConstPtr &command)
- {
- // data[0]=K5, data[1]=K6, data[2]=K7, data[3]=K8
- // data[4]=Pk5, data[5]=Pk6, data[6]=Pk7, data[7]=Pk8
- // F= [0 K5
- // K6 0]
- // Ginv=[0.5K7 0.5K8
- // 0.5K7 -0.5K8
-
- if(command->data[4] < 1e-3) F(0,1)=command->data[0];
- if(command->data[5] < 1e-3) F(1,0)=command->data[1];
- if(command->data[6] < 1e-3)
- {
- Ginv(0,0)=0.5*command->data[2];
- Ginv(1,0)=0.5*command->data[2];
- }
- if(command->data[7] < 1e-3)
- {
- Ginv(0,1)=0.5*command->data[3];
- Ginv(1,1)=-0.5*command->data[3];
- }
- }
-
-}
-
-PLUGINLIB_EXPORT_CLASS(twil_controllers::NonSmoothBackstepController,controller_interface::ControllerBase)
</description>
</class>
- <class name="twil_controllers/CartLinearizingController_RNE" type="twil_controllers::CartLinearizingController" base_class_type="controller_interface::ControllerBase">
- <description>
- The CartLinearizingController linearizes the Twil dynamic model. The
- linearized inputs are linear and angular accelerations. It expects a
- EffortJointInterface type of hardware interface. Recursive
- Newton-Euler from KDL is used to compute torque. Not working
- properly.
- </description>
- </class>
-
- <class name="twil_controllers/NonSmoothBackstepController" type="twil_controllers::NonSmoothBackstepController" base_class_type="controller_interface::ControllerBase">
- <description>
- The NonSmoothBackstepController implements a non-smooth controller
- based on a discontinuous transformation and backstepping. A feedback
- linearization of the Twil dynamic model is also performed. The
- inputs to the controller are the reference Cartesian pose. It expects a
- EffortJointInterface type of hardware interface.
- </description>
- </class>
-
</library>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
<robotNamespace>/twil</robotNamespace>
-
- <!-- Custom plugin -->
- <!-- robotSimType>twil_gazebo_ros_control/TwilRobotHWSim</robotSimType -->
-
- <!-- Default plugin -->
- <!-- robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType -->
-
<controlPeriod>0.001</controlPeriod>
</plugin>
</gazebo>
<launch>
- <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/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"/>
<include file="$(find twil_description)/launch/twil_sim.launch"/>
<rosparam file="$(find twil_controllers)/config/effort_control.yaml" command="load"/>
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="/twil"
- args="joint_state_controller left_wheel_joint_effort_controller right_wheel_joint_effort_controller"/>
+ <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+ output="screen" ns="/twil"
+ args="joint_state_controller left_wheel_joint_effort_controller \
+ right_wheel_joint_effort_controller"/>
<node name="ident" ns="/twil" pkg="twil_ident" type="ident" output="screen">
- <remap from="ident/left_wheel_command" to="left_wheel_command"/>
- <remap from="ident/right_wheel_command" to="right_wheel_command"/>
+ <remap from="ident/left_wheel_command" to="left_wheel_command"/>
+ <remap from="ident/right_wheel_command" to="right_wheel_command"/>
</node>
</launch>
return sh[index];
}
-
class Ident
{
public:
Eigen::MatrixXd P2;
std::vector<Prbs> prbs;
- int iter;
ros::Time lastTime;
void resetCovariance(void);
};
-
Ident::Ident(ros::NodeHandle node):
nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),prbs(nJoints)
{
P1*=1;
P2.setIdentity();
P2*=1;
- iter=0;
}
void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates)
dynParam.data.push_back(P2(i,i));
}
dynParamPublisher.publish(dynParam);
-
-// if(iter++ > 2048) resetCovariance();
}
void Ident::setCommand(void)
projects / twil.git / commitdiff