From: Walter Fetter Lages Date: Wed, 28 Nov 2018 13:25:20 +0000 (-0200) Subject: Initial commit. X-Git-Tag: rosbook2015initial~7 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=779345308cec705a67365a337eecc4725883ba0c;p=ufrgs_wam.git Initial commit. --- 779345308cec705a67365a337eecc4725883ba0c diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c1edc29 --- /dev/null +++ b/.gitignore @@ -0,0 +1,51 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +#*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..28105dd --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of +# directories (or patterns, but directories should suffice) that should +# be excluded from the distro. This is not the place to put things that +# should be ignored everywhere, like "build" directories; that happens in +# rosbuild/rosbuild.cmake. Here should be listed packages that aren't +# ready for inclusion in a distro. +# +# This list is combined with the list in rosbuild/rosbuild.cmake. Note +# that CMake 2.6 may be required to ensure that the two lists are combined +# properly. CMake 2.4 seems to have unpredictable scoping rules for such +# variables. +#list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) + +rosbuild_make_distribution(0.1.0) diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..a818cca --- /dev/null +++ b/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake_stack.mk \ No newline at end of file diff --git a/bhand_description/CMakeLists.txt b/bhand_description/CMakeLists.txt new file mode 100644 index 0000000..f8f1c9c --- /dev/null +++ b/bhand_description/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/bhand_description/Makefile b/bhand_description/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/bhand_description/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/bhand_description/launch/bhand.launch b/bhand_description/launch/bhand.launch new file mode 100644 index 0000000..1875c7c --- /dev/null +++ b/bhand_description/launch/bhand.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/bhand_description/launch/bhand_sim.launch b/bhand_description/launch/bhand_sim.launch new file mode 100644 index 0000000..09797be --- /dev/null +++ b/bhand_description/launch/bhand_sim.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/bhand_description/manifest.xml b/bhand_description/manifest.xml new file mode 100644 index 0000000..80a2f17 --- /dev/null +++ b/bhand_description/manifest.xml @@ -0,0 +1,14 @@ + + + + bhand_description + + + Walter Fetter Lages + GNU + + http://ros.org/wiki/bhand_description + + + + diff --git a/bhand_description/meshes/bh_base.stl b/bhand_description/meshes/bh_base.stl new file mode 100644 index 0000000..dce76f7 Binary files /dev/null and b/bhand_description/meshes/bh_base.stl differ diff --git a/bhand_description/meshes/bh_link1.stl b/bhand_description/meshes/bh_link1.stl new file mode 100644 index 0000000..bf35bd9 Binary files /dev/null and b/bhand_description/meshes/bh_link1.stl differ diff --git a/bhand_description/meshes/bh_link2.stl b/bhand_description/meshes/bh_link2.stl new file mode 100644 index 0000000..f74e529 Binary files /dev/null and b/bhand_description/meshes/bh_link2.stl differ diff --git a/bhand_description/meshes/bh_link3.stl b/bhand_description/meshes/bh_link3.stl new file mode 100644 index 0000000..d9361c6 Binary files /dev/null and b/bhand_description/meshes/bh_link3.stl differ diff --git a/bhand_description/xacro/bhand.urdf.xacro b/bhand_description/xacro/bhand.urdf.xacro new file mode 100644 index 0000000..b8fb615 --- /dev/null +++ b/bhand_description/xacro/bhand.urdf.xacro @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bhand_description/xacro/bhand0.urdf.xacro b/bhand_description/xacro/bhand0.urdf.xacro new file mode 100644 index 0000000..79f781a --- /dev/null +++ b/bhand_description/xacro/bhand0.urdf.xacro @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bhand_description/xacro/bhand_base.urdf.xacro b/bhand_description/xacro/bhand_base.urdf.xacro new file mode 100644 index 0000000..a397d3a --- /dev/null +++ b/bhand_description/xacro/bhand_base.urdf.xacro @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + Gazebo/Blue + + + + + + + + + + + + + + diff --git a/bhand_description/xacro/bhand_finger.urdf.xacro b/bhand_description/xacro/bhand_finger.urdf.xacro new file mode 100644 index 0000000..e8b7e88 --- /dev/null +++ b/bhand_description/xacro/bhand_finger.urdf.xacro @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + true + + + + + + + + + + + + + + + + + + + + + + + 1 + 1 + + + + + + diff --git a/plier_description/launch/display.launch b/plier_description/launch/display.launch new file mode 100644 index 0000000..a113ced --- /dev/null +++ b/plier_description/launch/display.launch @@ -0,0 +1,11 @@ + + + + + + + + + \ No newline at end of file diff --git a/plier_description/launch/gazebo.launch b/plier_description/launch/gazebo.launch new file mode 100644 index 0000000..c829339 --- /dev/null +++ b/plier_description/launch/gazebo.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/plier_description/launch/plier.launch b/plier_description/launch/plier.launch new file mode 100644 index 0000000..8376d88 --- /dev/null +++ b/plier_description/launch/plier.launch @@ -0,0 +1,4 @@ + + + + diff --git a/plier_description/launch/plier_sim.launch b/plier_description/launch/plier_sim.launch new file mode 100644 index 0000000..c9bf77f --- /dev/null +++ b/plier_description/launch/plier_sim.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/plier_description/manifest.xml b/plier_description/manifest.xml new file mode 100644 index 0000000..97a62e2 --- /dev/null +++ b/plier_description/manifest.xml @@ -0,0 +1,6 @@ + + plier_description + + Walter Fetter Lags + BSD + \ No newline at end of file diff --git a/plier_description/meshes/base_link.stl b/plier_description/meshes/base_link.stl new file mode 100644 index 0000000..eab1cf1 Binary files /dev/null and b/plier_description/meshes/base_link.stl differ diff --git a/plier_description/meshes/link1.stl b/plier_description/meshes/link1.stl new file mode 100644 index 0000000..6bcdc08 Binary files /dev/null and b/plier_description/meshes/link1.stl differ diff --git a/plier_description/meshes/link2.stl b/plier_description/meshes/link2.stl new file mode 100644 index 0000000..6f95350 Binary files /dev/null and b/plier_description/meshes/link2.stl differ diff --git a/plier_description/urdf/plier.urdf b/plier_description/urdf/plier.urdf new file mode 100644 index 0000000..6ad1514 --- /dev/null +++ b/plier_description/urdf/plier.urdf @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Yellow + + + + + Gazebo/White + + + + + Gazebo/Grey + + + diff --git a/stack.xml b/stack.xml new file mode 100644 index 0000000..c3783ed --- /dev/null +++ b/stack.xml @@ -0,0 +1,9 @@ + + UFRGS WAM + Maintained by Walter Fetter Lages + GNU + + http://ros.org/wiki/ufrgs_wam + + + diff --git a/wam_control_gazebo/CMakeLists.txt b/wam_control_gazebo/CMakeLists.txt new file mode 100644 index 0000000..97ce471 --- /dev/null +++ b/wam_control_gazebo/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) + +# Build RobotSim Interface +rosbuild_add_library(wam_control_gazebo src/robot_sim_wam.cpp) diff --git a/wam_control_gazebo/Makefile b/wam_control_gazebo/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/wam_control_gazebo/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/wam_control_gazebo/mainpage.dox b/wam_control_gazebo/mainpage.dox new file mode 100644 index 0000000..590fcdc --- /dev/null +++ b/wam_control_gazebo/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b twil_control_gazebo + + + +--> + + +*/ diff --git a/wam_control_gazebo/manifest.xml b/wam_control_gazebo/manifest.xml new file mode 100644 index 0000000..b8569ea --- /dev/null +++ b/wam_control_gazebo/manifest.xml @@ -0,0 +1,23 @@ + + + + wam_control_gazebo + + + Walter Fetter Lages + GPL + + http://ros.org/wiki/wam_control_gazebo + + + + + + + + + + + + + diff --git a/wam_control_gazebo/robot_sim_plugins.xml b/wam_control_gazebo/robot_sim_plugins.xml new file mode 100644 index 0000000..d52e012 --- /dev/null +++ b/wam_control_gazebo/robot_sim_plugins.xml @@ -0,0 +1,12 @@ + + + + + A ROS/Gazebo interface for Barrett WAM, exporting a joint_state_interface and a + joint_effort_interface. + + + diff --git a/wam_control_gazebo/src/robot_sim_wam.cpp b/wam_control_gazebo/src/robot_sim_wam.cpp new file mode 100644 index 0000000..5eead8e --- /dev/null +++ b/wam_control_gazebo/src/robot_sim_wam.cpp @@ -0,0 +1,102 @@ +#include + +#include +#include + +#include + +#include + +#include +#include +#include + +namespace wam_control_gazebo +{ + + class RobotSimWam:public ros_control_gazebo::RobotSim + { + + unsigned int n_dof_; + + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + + std::vector joint_name_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_effort_command_; + + std::vector sim_joints_; + + public: + + RobotSimWam(void):n_dof_(7),joint_name_(n_dof_),joint_position_(n_dof_), + joint_velocity_(n_dof_),joint_effort_(n_dof_),joint_effort_command_(n_dof_) + { + + joint_name_[0]="wam_joint_1"; + joint_name_[1]="wam_joint_2"; + joint_name_[2]="wam_joint_3"; + joint_name_[3]="wam_joint_4"; + joint_name_[4]="wam_joint_5"; + joint_name_[5]="wam_joint_6"; + joint_name_[6]="wam_joint_7"; + + 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_.registerJoint(joint_name_[j],&joint_position_[j],&joint_velocity_[j],&joint_effort_[j]); + ej_interface_.registerJoint(js_interface_.getJointStateHandle(joint_name_[j]),&joint_effort_command_[j]); + } + + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + } + + + bool initSim(ros::NodeHandle nh,gazebo::physics::ModelPtr model) + { + 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).GetAsRadian(); + joint_velocity_[j]=sim_joints_[j]->GetVelocity(0); +// joint_effort_[j]=sim_joints_[j]->GetForce(0u); + 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_DECLARE_CLASS(wam_control_gazebo,RobotSimWam,wam_control_gazebo::RobotSimWam,ros_control_gazebo::RobotSim) diff --git a/wam_controllers/CMakeLists.txt b/wam_controllers/CMakeLists.txt new file mode 100644 index 0000000..95ad1f8 --- /dev/null +++ b/wam_controllers/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +rosbuild_add_library(${PROJECT_NAME} src/computed_torque_controller.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/wam_controllers/Makefile b/wam_controllers/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/wam_controllers/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/wam_controllers/README b/wam_controllers/README new file mode 100644 index 0000000..b29cdbe --- /dev/null +++ b/wam_controllers/README @@ -0,0 +1,10 @@ +To publish reference: + +rostopic pub /wam/computed_torque_controller/command trajectory_msgs/JointTrajectoryPoint "[0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" "[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]" "[0.0, 0.0]" + +Arguments are positions, velocities, accelerations, 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/config/computed_torque_control.yaml b/wam_controllers/config/computed_torque_control.yaml new file mode 100644 index 0000000..a1e546f --- /dev/null +++ b/wam_controllers/config/computed_torque_control.yaml @@ -0,0 +1,16 @@ +wam: + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + computed_torque_controller: + type: wam_controllers/ComputedTorqueController + joints: + - wam_joint_1 + - wam_joint_2 + - wam_joint_3 + - wam_joint_4 + - wam_joint_5 + - wam_joint_6 + - wam_joint_7 diff --git a/wam_controllers/doc/urdf2kdl.txt b/wam_controllers/doc/urdf2kdl.txt new file mode 100644 index 0000000..0f5af05 --- /dev/null +++ b/wam_controllers/doc/urdf2kdl.txt @@ -0,0 +1,9 @@ +KDL specifies the inertia in the reference frame of the link, the URDF + specifies the inertia in the inertia reference frame. + +A KDL segment is an ideal rigid body to which one single Joint is connected +and one single tip frame. It contains: + +- a Joint located at the root frame of the Segment. +- a Frame describing the pose between the end of the Joint and the tip +frame of the Segment. diff --git a/wam_controllers/include/wam_controllers/computed_torque_controller.h b/wam_controllers/include/wam_controllers/computed_torque_controller.h new file mode 100644 index 0000000..60a09ea --- /dev/null +++ b/wam_controllers/include/wam_controllers/computed_torque_controller.h @@ -0,0 +1,61 @@ +#ifndef WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H +#define WAM_CONTROLLERS_COMPUTED_TORQUE_CONTROLLER_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + + +namespace wam_controllers +{ + class ComputedTorqueController: public controller_interface::Controller + { + public: + ComputedTorqueController(void); + ~ComputedTorqueController(void); + + bool init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n); + void starting(const ros::Time& time); + void update(const ros::Time& time); + + private: + ros::NodeHandle node_; + ros::Time last_time_; + hardware_interface::EffortJointInterface *robot_; + std::vector joints_; + + void commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr &referencePoint); + ros::Subscriber sub_command_; + + KDL::ChainIdSolver_RNE *idsolver; + + KDL::JntArray q; + KDL::JntArray dq; + KDL::JntArray ddq; + + KDL::JntArray qr; + KDL::JntArray dqr; + KDL::JntArray ddqr; + + KDL::JntArray torque; + + KDL::Wrenches fext; + + Eigen::MatrixXd Kp; + Eigen::MatrixXd Kd; + + }; +} +#endif diff --git a/wam_controllers/launch/computed_torque.launch b/wam_controllers/launch/computed_torque.launch new file mode 100644 index 0000000..2da9e12 --- /dev/null +++ b/wam_controllers/launch/computed_torque.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/wam_controllers/launch/computed_torque_table.launch b/wam_controllers/launch/computed_torque_table.launch new file mode 100644 index 0000000..3975bd5 --- /dev/null +++ b/wam_controllers/launch/computed_torque_table.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/wam_controllers/mainpage.dox b/wam_controllers/mainpage.dox new file mode 100644 index 0000000..deb448e --- /dev/null +++ b/wam_controllers/mainpage.dox @@ -0,0 +1,14 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b twil_controllers + + + +--> + + +*/ diff --git a/wam_controllers/manifest.xml b/wam_controllers/manifest.xml new file mode 100644 index 0000000..2fd288c --- /dev/null +++ b/wam_controllers/manifest.xml @@ -0,0 +1,25 @@ + + + + wam_controllers + + + Walter Fetter Lages + GPL + + http://ros.org/wiki/wam_controllers + + + + + + + + + + + + + + + diff --git a/wam_controllers/src/computed_torque_controller.cpp b/wam_controllers/src/computed_torque_controller.cpp new file mode 100644 index 0000000..407a982 --- /dev/null +++ b/wam_controllers/src/computed_torque_controller.cpp @@ -0,0 +1,173 @@ +#include +#include + +#include +#include +#include + +#define Ts 5.0 +#define Xi 1.0 +#define Wn (4.0/Ts/Xi) + + +namespace wam_controllers +{ + ComputedTorqueController::ComputedTorqueController(void): + q(0),dq(0),ddq(0),qr(0),dqr(0),ddqr(0),torque(0),fext(0) + { + } + + ComputedTorqueController::~ComputedTorqueController(void) + { + sub_command_.shutdown(); + } + + bool ComputedTorqueController::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->getJointHandle((std::string)name_value); + joints_.push_back(j); + } + sub_command_ = node_.subscribe("command",1000,&ComputedTorqueController::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("wam_origin","wam_tool_plate",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[0],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; + } + + // set vectors to the right size + q.resize(chain.getNrOfJoints()); + dq.resize(chain.getNrOfJoints()); + ddq.resize(chain.getNrOfJoints()); + qr.resize(chain.getNrOfJoints()); + dqr.resize(chain.getNrOfJoints()); + ddqr.resize(chain.getNrOfJoints()); + torque.resize(chain.getNrOfJoints()); + + fext.resize(chain.getNrOfSegments()); + + Kp.resize(chain.getNrOfJoints(),chain.getNrOfJoints()); + Kd.resize(chain.getNrOfJoints(),chain.getNrOfJoints()); + + return true; + } + + void ComputedTorqueController::starting(const ros::Time& time) + { + last_time_=time; + Kp.setZero(); + Kd.setZero(); + for(unsigned int i=0; i < joints_.size();i++) + { + q(i)=joints_[i].getPosition(); + dq(i)=joints_[i].getVelocity(); + Kp(i,i)=Wn*Wn; + Kd(i,i)=2.0*Xi*Wn; + } + qr=q; + dqr=dq; + SetToZero(ddqr); + } + + void ComputedTorqueController::update(const ros::Time& time) + { + ros::Duration dt=time-last_time_; + last_time_=time; + + for(unsigned int i=0;i < joints_.size();i++) + { + q(i)=joints_[i].getPosition(); + dq(i)=joints_[i].getVelocity(); + } + + for(unsigned int i=0;i < fext.size();i++) fext[i].Zero(); + + ddq.data=ddqr.data+Kp*(qr.data-q.data)+Kd*(dqr.data-dq.data); + +/* std::cout << "time=" << time.toSec() << std::endl; + + for(unsigned int i=0;i < joints_.size();i++) + { + std::cout << "q[" << i << "]=" << q(i) + << "\tqr[" << i << "]=" << qr(i) << std::endl; + + std::cout << "dq[" << i << "]=" << dq(i) + << "\tdqr[" << i << "]=" << dqr(i) << std::endl; + + + std::cout << "ddq[" << i << "]=" << ddq(i) + << "\tddqr[" << i << "]=" << ddqr(i) << std::endl; + } +*/ + // Compute linearization. + if(idsolver->CartToJnt(q,dq,ddq,fext,torque) < 0) ROS_ERROR("KDL inverse dynamics solver failed."); + +// for(unsigned int i=0;i < joints_.size();i++) std::cout << "torque[" << i << "]=" << torque(i) << std::endl; + + // Apply torques + for(unsigned int i=0;i < joints_.size();i++) joints_[i].setCommand(torque(i)); + } + + void ComputedTorqueController::commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr &referencePoint) + { + for(unsigned int i=0;i < qr.rows();i++) + { + qr(i)=referencePoint->positions[i]; + dqr(i)=referencePoint->velocities[i]; + ddqr(i)=referencePoint->accelerations[i]; + } + } +} +PLUGINLIB_DECLARE_CLASS(wam_controllers,ComputedTorqueController,wam_controllers::ComputedTorqueController,controller_interface::ControllerBase) diff --git a/wam_controllers/wam_controllers_plugins.xml b/wam_controllers/wam_controllers_plugins.xml new file mode 100644 index 0000000..4be4031 --- /dev/null +++ b/wam_controllers/wam_controllers_plugins.xml @@ -0,0 +1,11 @@ + + + + + The ComputedTorqueControllers linearizes the Barrett WAM dynamic + model. The linearized inputs are joint accelerations. It expects a + EffortJointInterface type of hardware interface. + + + + diff --git a/wam_description/CMakeLists.txt b/wam_description/CMakeLists.txt new file mode 100644 index 0000000..f8f1c9c --- /dev/null +++ b/wam_description/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +#set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_add_boost_directories() +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/wam_description/Makefile b/wam_description/Makefile new file mode 100644 index 0000000..b75b928 --- /dev/null +++ b/wam_description/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/wam_description/launch/wam.launch b/wam_description/launch/wam.launch new file mode 100644 index 0000000..e13ade7 --- /dev/null +++ b/wam_description/launch/wam.launch @@ -0,0 +1,4 @@ + + + + diff --git a/wam_description/launch/wam_bhand.launch b/wam_description/launch/wam_bhand.launch new file mode 100644 index 0000000..5302c4b --- /dev/null +++ b/wam_description/launch/wam_bhand.launch @@ -0,0 +1,4 @@ + + + + diff --git a/wam_description/launch/wam_bhand_sim.launch b/wam_description/launch/wam_bhand_sim.launch new file mode 100644 index 0000000..60b4beb --- /dev/null +++ b/wam_description/launch/wam_bhand_sim.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/wam_description/launch/wam_sim.launch b/wam_description/launch/wam_sim.launch new file mode 100644 index 0000000..b22b8d0 --- /dev/null +++ b/wam_description/launch/wam_sim.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/wam_description/launch/wam_table.launch b/wam_description/launch/wam_table.launch new file mode 100644 index 0000000..fb882a6 --- /dev/null +++ b/wam_description/launch/wam_table.launch @@ -0,0 +1,4 @@ + + + + diff --git a/wam_description/launch/wam_table_sim.launch b/wam_description/launch/wam_table_sim.launch new file mode 100644 index 0000000..05648af --- /dev/null +++ b/wam_description/launch/wam_table_sim.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/wam_description/manifest.xml b/wam_description/manifest.xml new file mode 100644 index 0000000..44032f1 --- /dev/null +++ b/wam_description/manifest.xml @@ -0,0 +1,14 @@ + + + + wam_description + + + Walter Fetter Lages + GNU + + http://ros.org/wiki/wam_description + + + + diff --git a/wam_description/meshes/wam1.stl b/wam_description/meshes/wam1.stl new file mode 100644 index 0000000..3c8a5a4 Binary files /dev/null and b/wam_description/meshes/wam1.stl differ diff --git a/wam_description/meshes/wam2.stl b/wam_description/meshes/wam2.stl new file mode 100644 index 0000000..0f50c93 Binary files /dev/null and b/wam_description/meshes/wam2.stl differ diff --git a/wam_description/meshes/wam3.stl b/wam_description/meshes/wam3.stl new file mode 100644 index 0000000..966efcc Binary files /dev/null and b/wam_description/meshes/wam3.stl differ diff --git a/wam_description/meshes/wam4.stl b/wam_description/meshes/wam4.stl new file mode 100644 index 0000000..89c9de2 Binary files /dev/null and b/wam_description/meshes/wam4.stl differ diff --git a/wam_description/meshes/wam5.stl b/wam_description/meshes/wam5.stl new file mode 100644 index 0000000..97cb0ab Binary files /dev/null and b/wam_description/meshes/wam5.stl differ diff --git a/wam_description/meshes/wam6.stl b/wam_description/meshes/wam6.stl new file mode 100644 index 0000000..e3b014a Binary files /dev/null and b/wam_description/meshes/wam6.stl differ diff --git a/wam_description/meshes/wam7.stl b/wam_description/meshes/wam7.stl new file mode 100644 index 0000000..2cbf2b1 Binary files /dev/null and b/wam_description/meshes/wam7.stl differ diff --git a/wam_description/meshes/wambase.stl b/wam_description/meshes/wambase.stl new file mode 100644 index 0000000..f7d5132 Binary files /dev/null and b/wam_description/meshes/wambase.stl differ diff --git a/wam_description/xacro/.wam_j1.urdf.xacro.swo b/wam_description/xacro/.wam_j1.urdf.xacro.swo new file mode 100644 index 0000000..c20cc44 Binary files /dev/null and b/wam_description/xacro/.wam_j1.urdf.xacro.swo differ diff --git a/wam_description/xacro/wam.urdf.xacro b/wam_description/xacro/wam.urdf.xacro new file mode 100644 index 0000000..6fae73e --- /dev/null +++ b/wam_description/xacro/wam.urdf.xacro @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + wam + wam_control_gazebo/RobotSimWam + 0.001 + + + + + diff --git a/wam_description/xacro/wam_base.urdf.xacro b/wam_description/xacro/wam_base.urdf.xacro new file mode 100644 index 0000000..3091252 --- /dev/null +++ b/wam_description/xacro/wam_base.urdf.xacro @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + Gazebo/Grey + + + + + + + + + + + + + diff --git a/wam_description/xacro/wam_bhand.urdf.xacro b/wam_description/xacro/wam_bhand.urdf.xacro new file mode 100644 index 0000000..f9bec07 --- /dev/null +++ b/wam_description/xacro/wam_bhand.urdf.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + diff --git a/wam_description/xacro/wam_j1.urdf.xacro b/wam_description/xacro/wam_j1.urdf.xacro new file mode 100644 index 0000000..556b2b7 --- /dev/null +++ b/wam_description/xacro/wam_j1.urdf.xacro @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + true + + + + + + + + + + + + + + + + + 1 + 1 + + + + + diff --git a/wam_description/xacro/wam_j2.urdf.xacro b/wam_description/xacro/wam_j2.urdf.xacro new file mode 100644 index 0000000..0e67dcc --- /dev/null +++ b/wam_description/xacro/wam_j2.urdf.xacro @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + true + + + + + + + + + + + + + + + + 1 + 1 + + + + + + diff --git a/wam_description/xacro/wam_j3.urdf.xacro b/wam_description/xacro/wam_j3.urdf.xacro new file mode 100644 index 0000000..40973a7 --- /dev/null +++ b/wam_description/xacro/wam_j3.urdf.xacro @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + true + + + + + + + + + + + + + + + + 1 + 1 + + + + + + diff --git a/wam_description/xacro/wam_j4.urdf.xacro b/wam_description/xacro/wam_j4.urdf.xacro new file mode 100644 index 0000000..c357a29 --- /dev/null +++ b/wam_description/xacro/wam_j4.urdf.xacro @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + true + + + + + + + + + + + + + + + + 1 + 1 + + + + + + diff --git a/wam_description/xacro/wam_j5.urdf.xacro b/wam_description/xacro/wam_j5.urdf.xacro new file mode 100644 index 0000000..8a31b40 --- /dev/null +++ b/wam_description/xacro/wam_j5.urdf.xacro @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + true + + + + + + + + + + + + + + + + 1 + 1 + + + + + + diff --git a/wam_description/xacro/wam_j6.urdf.xacro b/wam_description/xacro/wam_j6.urdf.xacro new file mode 100644 index 0000000..e40a5e1 --- /dev/null +++ b/wam_description/xacro/wam_j6.urdf.xacro @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + true + + + + + + + + + + + + + + + + 1 + 1 + + + + + + diff --git a/wam_description/xacro/wam_j7.urdf.xacro b/wam_description/xacro/wam_j7.urdf.xacro new file mode 100644 index 0000000..246a7a3 --- /dev/null +++ b/wam_description/xacro/wam_j7.urdf.xacro @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + true + + + + + + + + + + + + + + + + 1 + 1 + + + + + + diff --git a/wam_description/xacro/wam_table.urdf.xacro b/wam_description/xacro/wam_table.urdf.xacro new file mode 100644 index 0000000..ddb7493 --- /dev/null +++ b/wam_description/xacro/wam_table.urdf.xacro @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wam_description/xacro/wam_tool_plate.urdf.xacro b/wam_description/xacro/wam_tool_plate.urdf.xacro new file mode 100644 index 0000000..1530ea9 --- /dev/null +++ b/wam_description/xacro/wam_tool_plate.urdf.xacro @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + true + + + + + + + + + + + +