Add Copyright notes.
<name>ufrgs_wam</name>
<version>2.0.0</version>
<description>The ufrgs_wam 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 -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages
+ </maintainer>
<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/ufrgs_wam</url> -->
<url type="website">http://www.ece.ufrgs.br/~fetter/ufrgs_wam</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> -->
-
-
- <!-- 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>
<run_depend>wam_description</run_depend>
- <run_depend>wam_gazebo_ros_control</run_depend>
<run_depend>wam_controllers</run_depend>
- <!-- The export tag contains other, unspecified, tags -->
<export>
- <!-- You can specify that this package is a metapackage here: -->
- <!-- <metapackage/> -->
- <metapackage/>
-
- <!-- Other tools can request additional information be placed here -->
-
+ <metapackage/>
</export>
</package>
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
+/******************************************************************************
+ UFRGS WAM
+ Computed Torque Controller
+ Copyright (C) 2013-2015 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
#ifndef WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H
#define WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H
<name>wam_controllers</name>
<version>2.0.0</version>
<description>The wam_controllers 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 -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages
+ </maintainer>
<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/wam_controllers</url> -->
<url type="website">http://www.ece.ufrgs.br/ufrgs_wam</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>wam_gazebo_ros_control</build_depend>
<build_depend>controller_interface</build_depend>
<build_depend>orocos_kdl</build_depend>
<build_depend>kdl_parser</build_depend>
<run_depend>controller_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>control_msgs</run_depend>
- <run_depend> joint_state_controller</run_depend>
+ <run_depend>joint_state_controller</run_depend>
<run_depend>urdf</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 -->
<controller_interface plugin="${prefix}/wam_controllers_plugins.xml"/>
</export>
-</package>
\ No newline at end of file
+
+</package>
+/******************************************************************************
+ UFRGS WAM
+ Computed Torque Controller
+ Copyright (C) 2013-2015 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <sys/mman.h>
+
#include <wam_controllers/computed_torque_controller.h>
#include <pluginlib/class_list_macros.h>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
-#define Ts 0.5
+#define Ts 0.8
#define Xi 1.0
#define Wn (4.0/Ts/Xi)
{
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;
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++)
{
}
}
-PLUGINLIB_EXPORT_CLASS(wam_controllers::ComputedTorqueController,controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(wam_controllers::ComputedTorqueController,
+ controller_interface::ControllerBase)
<launch>
- <param name="robot_description" command="$(find xacro)/xacro.py '$(find wam_description)/xacro/wam.urdf.xacro'" />
+ <param name="robot_description" command="$(find xacro)/xacro.py '$(find wam_description)/xacro/wam_world.urdf.xacro'" />
<node name="wam_spawner" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model wam" respawn="false" output="screen" />
</launch>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" >
<robotNamespace>/wam</robotNamespace>
-
- <!-- Custom plugin -->
- <!-- robotSimType>wam_gazebo_ros_control/WamRobotHWSim</robotSimType -->
-
- <!-- Default plugin -->
- <!-- robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType -->
-
+ <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<controlPeriod>0.001</controlPeriod>
</plugin>
</gazebo>
--- /dev/null
+<?xml version="1.0"?>
+
+<robot name ="wam" xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <link name="world" />
+
+ <xacro:include filename="$(find wam_description)/xacro/wam.urdf.xacro" />
+
+ <joint name="world_wam_joint" type="fixed" static="true">
+ <parent link="world"/>
+ <child link="wam_origin" />
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+ </joint>
+
+</robot>
projects / ufrgs_wam.git / commitdiff