From: Walter Fetter Lages Date: Wed, 28 Nov 2018 18:41:17 +0000 (-0200) Subject: Add PREEMPT_RT initialization to computed_torque_controller.cpp. X-Git-Tag: rosbook2015initial~1 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=318f484ba75df116f0c5b307f58d85d75ffdb426;p=ufrgs_wam.git Add PREEMPT_RT initialization to computed_torque_controller.cpp. Add Copyright notes. --- 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 c20cc44..0000000 Binary files a/wam_description/xacro/.wam_j1.urdf.xacro.swo and /dev/null differ diff --git a/wam_description/xacro/wam.urdf.xacro b/wam_description/xacro/wam.urdf.xacro index c0db1f6..7a2f79f 100644 --- a/wam_description/xacro/wam.urdf.xacro +++ b/wam_description/xacro/wam.urdf.xacro @@ -35,13 +35,7 @@ /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 @@ + + + + + + + + + + + + + + +