From 318f484ba75df116f0c5b307f58d85d75ffdb426 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 28 Nov 2018 16:41:17 -0200 Subject: [PATCH] Add PREEMPT_RT initialization to computed_torque_controller.cpp. Add Copyright notes. --- ufrgs_wam/package.xml | 45 ++------------------ wam_controllers/README | 3 -- .../wam_controllers/computed_torque_controller.h | 21 ++++++++++ wam_controllers/package.xml | 46 +++------------------ wam_controllers/src/computed_torque_controller.cpp | 43 +++++++++++++++++-- wam_description/launch/wam.launch | 2 +- wam_description/xacro/.wam_j1.urdf.xacro.swo | Bin 12288 -> 0 bytes wam_description/xacro/wam.urdf.xacro | 8 +--- wam_description/xacro/wam_world.urdf.xacro | 15 +++++++ 9 files changed, 85 insertions(+), 98 deletions(-) delete mode 100644 wam_description/xacro/.wam_j1.urdf.xacro.swo create mode 100644 wam_description/xacro/wam_world.urdf.xacro diff --git a/ufrgs_wam/package.xml b/ufrgs_wam/package.xml index 9606051..2bbf3d1 100644 --- a/ufrgs_wam/package.xml +++ b/ufrgs_wam/package.xml @@ -3,56 +3,17 @@ ufrgs_wam 2.0.0 The ufrgs_wam package - - - - - Walter Fetter Lages - - - - - + Walter Fetter Lages + GPLv3 - - - - - - http://www.ece.ufrgs.br/~fetter/ufrgs_wam - - - - - - - - - - - - - - - - - - catkin wam_description - wam_gazebo_ros_control wam_controllers - - - - - - - + diff --git a/wam_controllers/README b/wam_controllers/README index 861d535..07f6e1b 100644 --- a/wam_controllers/README +++ b/wam_controllers/README @@ -5,6 +5,3 @@ rostopic pub /wam/computed_torque_controller/command trajectory_msgs/JointTrajec Arguments are positions, velocities, accelerations, effort, time from start in seconds and nanoseconds. -Set starting position: - -rosservice call /gazebo/set_model_configuration wam joint ['wam_joint_1','wam_joint_2','wam_joint_3','wam_joint_4','wam_joint_5','wam_joint_6','wam_joint_7'] [0.0,0.75,0.0,1.5,0.0,0.9,0.0] \ No newline at end of file diff --git a/wam_controllers/include/wam_controllers/computed_torque_controller.h b/wam_controllers/include/wam_controllers/computed_torque_controller.h index 2bbbe22..6abd28c 100644 --- a/wam_controllers/include/wam_controllers/computed_torque_controller.h +++ b/wam_controllers/include/wam_controllers/computed_torque_controller.h @@ -1,3 +1,24 @@ +/****************************************************************************** + UFRGS WAM + Computed Torque Controller + Copyright (C) 2013-2015 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + #ifndef WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H #define WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H diff --git a/wam_controllers/package.xml b/wam_controllers/package.xml index 118d020..67e159c 100644 --- a/wam_controllers/package.xml +++ b/wam_controllers/package.xml @@ -3,46 +3,14 @@ wam_controllers 2.0.0 The wam_controllers package - - - - - Walter Fetter Lages - - - - - + Walter Fetter Lages + GPLv3 - - - - - - http://www.ece.ufrgs.br/ufrgs_wam - - - - - - Walter Fetter Lages - - - - - - - - - - - catkin - wam_gazebo_ros_control controller_interface orocos_kdl kdl_parser @@ -55,16 +23,12 @@ controller_interface controller_manager control_msgs - joint_state_controller + joint_state_controller urdf kdl_parser - - - - - - \ No newline at end of file + + diff --git a/wam_controllers/src/computed_torque_controller.cpp b/wam_controllers/src/computed_torque_controller.cpp index f2a4482..de09786 100644 --- a/wam_controllers/src/computed_torque_controller.cpp +++ b/wam_controllers/src/computed_torque_controller.cpp @@ -1,3 +1,26 @@ +/****************************************************************************** + UFRGS WAM + Computed Torque Controller + Copyright (C) 2013-2015 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#include + #include #include @@ -5,7 +28,7 @@ #include #include -#define Ts 0.5 +#define Ts 0.8 #define Xi 1.0 #define Wn (4.0/Ts/Xi) @@ -111,7 +134,7 @@ namespace wam_controllers { Kp.setZero(); Kd.setZero(); - for(unsigned int i=0; i < joints_.size();i++) + for(unsigned int i=0;i < joints_.size();i++) { Kp(i,i)=Wn*Wn; Kd(i,i)=2.0*Xi*Wn; @@ -121,9 +144,20 @@ namespace wam_controllers qr=q; dqr=dq; SetToZero(ddqr); + + struct sched_param param; + param.sched_priority=sched_get_priority_max(SCHED_FIFO); + if(sched_setscheduler(0,SCHED_FIFO,¶m) == -1) + { + ROS_WARN("Failed to set real-time scheduler."); + return; + } + if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) + ROS_WARN("Failed to lock memory."); } - void ComputedTorqueController::update(const ros::Time& time,const ros::Duration& duration) + void ComputedTorqueController::update(const ros::Time& time, + const ros::Duration& duration) { for(unsigned int i=0;i < joints_.size();i++) { @@ -152,4 +186,5 @@ namespace wam_controllers } } -PLUGINLIB_EXPORT_CLASS(wam_controllers::ComputedTorqueController,controller_interface::ControllerBase) +PLUGINLIB_EXPORT_CLASS(wam_controllers::ComputedTorqueController, + controller_interface::ControllerBase) diff --git a/wam_description/launch/wam.launch b/wam_description/launch/wam.launch index b783aa0..2cac088 100644 --- a/wam_description/launch/wam.launch +++ b/wam_description/launch/wam.launch @@ -1,4 +1,4 @@ - + diff --git a/wam_description/xacro/.wam_j1.urdf.xacro.swo b/wam_description/xacro/.wam_j1.urdf.xacro.swo deleted file mode 100644 index c20cc44b5015db357a8c3c15572928edd753427e..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 12288 zcmeI2zmFS56vroNNC*K01rmzHu+ZM_+RmMm)7mK_5h4*GK>|V))_6U(=e)m|ojuzJ z2?YX)hK7;`rJ{fiB>n`1geW02)C7qV2~ogzW_NvF=f@$ah-Rg)cm3wg%$v`9JJ#uD z=)ZRP3hlZbg6nZYe)onie|-E~a`S0IW?8JQM_#jK#Ix{7H*y05dNR$uIg1Cmh|PV( z^H7RRi8S#RER?CS?zP-Pj>fLB96hr2ummiD`w}SBc*M84A&(bHKxOU$z zw$T!>1S|ndz!IGgdB*~=2J1S|nd zz!Is!&LPNp)@GZ6Yj?PRH3nGo02TaW0B`LaM*{JzJfSasl1xXw+enS zO>)H&)o%sf-OR-FKo=fPSt3Ffy~0ODsLwB&E_X54UG`FuD4MXC_Z)o$a<4ToCWm^f zNLZ)}hF~f;xR@YTF2Q>IR4p@%mC5#?3MY@@KjM-1s))0QUtL?Q)0}yI9H*iKJSLzz z8ju++5w8eyI%07ql1a~L`M$G?7aoy_g`!b9=d$N)wcQ z-g=@5Ph+m+@^G?MTg>w*9gB#U)t<3%#wL8%yW_~OcI4(naLdhA%6xM_oXr)Fr zP2}~&&5B;Fvm&tyrddCz4~%WrWokzkTw`$na|f`%+ZTC()R1~q6Uatx8f_Q_ud$pA z8Q{6yZA@8zXuxX~YKE$_T&?Mngq?0vmuyCyaUn(G^R@5>`{lcs#To;L%t&@&A(= zSr56&S)S8<78SZ+k=~u|&N+NE3N~o21lwgYYt(JIaRlb1d-+?{Y^K>=H#Swcql@o6+W}^HJG@UNMA6u~0TR} /wam - - - - - - - + gazebo_ros_control/DefaultRobotHWSim 0.001 diff --git a/wam_description/xacro/wam_world.urdf.xacro b/wam_description/xacro/wam_world.urdf.xacro new file mode 100644 index 0000000..22f2a36 --- /dev/null +++ b/wam_description/xacro/wam_world.urdf.xacro @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + -- 2.12.0