Change license to GPL.
Add scripts to set initial conditions.
Add scripts to move the robot.
<stack>
<description brief="ufrgs_wam">UFRGS WAM</description>
<author>Maintained by Walter Fetter Lages</author>
- <license>GNU</license>
+ <license>GPL</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/ufrgs_wam</url>
<depend stack="ros" />
-
</stack>
<depend package="ros_control_gazebo"/>
<depend package="ros_control_gazebo_plugin"/>
-
<depend package="wam_description"/>
-
<depend package="gazebo"/>
<export>
<ros_control_gazebo plugin="${prefix}/robot_sim_plugins.xml" />
</export>
-
</package>
type="wam_control_gazebo::RobotSimWam"
base_class_type="ros_control_gazebo::RobotSim">
<description>
- A ROS/Gazebo interface for Barrett WAM, exporting a joint_state_interface and a
- joint_effort_interface.
+ A ROS/Gazebo interface for Barrett WAM, exporting a
+ joint_state_interface and a joint_effort_interface.
</description>
</class>
+
</library>
namespace wam_control_gazebo
{
-
class RobotSimWam:public ros_control_gazebo::RobotSim
{
-
unsigned int n_dof_;
hardware_interface::JointStateInterface js_interface_;
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_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]);
- 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]);
+ joint_effort_command_[j] = 0.0;
+ ej_interface_.registerJoint(js_interface_.getJointStateHandle(joint_name_[j]),
+ &joint_effort_command_[j]);
}
registerInterface(&js_interface_);
{
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]);
+ 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)
+PLUGINLIB_DECLARE_CLASS(wam_control_gazebo,RobotSimWam,wam_control_gazebo::RobotSimWam,
+ ros_control_gazebo::RobotSim)
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
-
namespace wam_controllers
{
- class ComputedTorqueController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
+ class ComputedTorqueController: public controller_interface::
+ Controller<hardware_interface::EffortJointInterface>
{
- 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<hardware_interface::JointHandle> joints_;
-
- void commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr &referencePoint);
- ros::Subscriber sub_command_;
+ ros::NodeHandle node_;
+
+ hardware_interface::EffortJointInterface *robot_;
+ std::vector<hardware_interface::JointHandle> joints_;
+
+ ros::Subscriber sub_command_;
- KDL::ChainIdSolver_RNE *idsolver;
+ KDL::ChainIdSolver_RNE *idsolver;
- KDL::JntArray q;
- KDL::JntArray dq;
- KDL::JntArray ddq;
+ KDL::JntArray q;
+ KDL::JntArray dq;
+ KDL::JntArray v;
- KDL::JntArray qr;
- KDL::JntArray dqr;
- KDL::JntArray ddqr;
+ KDL::JntArray qr;
+ KDL::JntArray dqr;
+ KDL::JntArray ddqr;
- KDL::JntArray torque;
+ KDL::JntArray torque;
- KDL::Wrenches fext;
-
- Eigen::MatrixXd Kp;
- Eigen::MatrixXd Kd;
+ KDL::Wrenches fext;
+ Eigen::MatrixXd Kp;
+ Eigen::MatrixXd Kd;
+
+ void commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr
+ &referencePoint);
+
+ 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);
};
}
#endif
<arg name="paused" value="$(arg paused)"/>
</include>
- <rosparam file="$(find wam_controllers)/config/computed_torque_control.yaml" command="load"/>
+ <rosparam file="$(find wam_controllers)/config/computed_torque_control.yaml"
+ command="load"/>
- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
- output="screen" ns="/wam" args="joint_state_controller computed_torque_controller"/>
+ <node name="controller_spawner" pkg="controller_manager" type="spawner"
+ respawn="false" output="screen" ns="/wam"
+ args="joint_state_controller computed_torque_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" />
</launch>
<license>GPL</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/wam_controllers</url>
+
<depend package="joint_state_controller"/>
- <!--depend package="effort_controllers"/-->
<depend package="wam_control_gazebo"/>
<depend package="controller_interface"/>
<depend package="orocos_kdl"/>
</export>
</package>
-
-
--- /dev/null
+#!/bin/bash
+
+rostopic pub /wam/computed_torque_controller/command \
+trajectory_msgs/JointTrajectoryPoint \
+"[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, 0.0]" -1
--- /dev/null
+#!/bin/bash
+
+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,-2.0,0.0,3.1,0.0,0.0,0.0]
+
+rostopic pub /wam/computed_torque_controller/command \
+trajectory_msgs/JointTrajectoryPoint \
+"[0.0,-2.0,0.0,3.1,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]" -1
--- /dev/null
+#!/bin/bash
+
+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]
+
+rostopic pub /wam/computed_torque_controller/command \
+trajectory_msgs/JointTrajectoryPoint \
+"[0.0,0.75,0.0,1.5,0.0,0.9,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]" -1
#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)
+ q(0),dq(0),v(0),qr(0),dqr(0),ddqr(0),torque(0),fext(0)
{
}
sub_command_.shutdown();
}
- bool ComputedTorqueController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
+ 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());
+ ROS_ERROR("No 'joints' 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());
+ ROS_ERROR("'joints' is not a struct. (namespace: %s)",
+ node_.getNamespace().c_str());
return false;
}
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());
+ ROS_ERROR("joints are not strings. (namespace: %s)",
+ node_.getNamespace().c_str());
return false;
}
- hardware_interface::JointHandle j=robot->getJointHandle((std::string)name_value);
+ hardware_interface::JointHandle j=robot->
+ getJointHandle((std::string)name_value);
joints_.push_back(j);
}
- sub_command_ = node_.subscribe("command",1000,&ComputedTorqueController::commandCB, this);
+ sub_command_=node_.subscribe("command",1000,
+ &ComputedTorqueController::commandCB, this);
std::string robot_desc_string;
if(!node_.getParam("/robot_description",robot_desc_string))
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);
return false;
}
- // set vectors to the right size
q.resize(chain.getNrOfJoints());
dq.resize(chain.getNrOfJoints());
- ddq.resize(chain.getNrOfJoints());
+ v.resize(chain.getNrOfJoints());
qr.resize(chain.getNrOfJoints());
dqr.resize(chain.getNrOfJoints());
ddqr.resize(chain.getNrOfJoints());
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;
+ q(i)=joints_[i].getPosition();
+ dq(i)=joints_[i].getVelocity();
}
qr=q;
dqr=dq;
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;
+ v.data=ddqr.data+Kp*(qr.data-q.data)+Kd*(dqr.data-dq.data);
+ if(idsolver->CartToJnt(q,dq,v,fext,torque) < 0)
+ ROS_ERROR("KDL inverse dynamics solver failed.");
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));
+ joints_[i].setCommand(torque(i));
}
- void ComputedTorqueController::commandCB(const trajectory_msgs::JointTrajectoryPoint::ConstPtr &referencePoint)
+ void ComputedTorqueController::commandCB(const trajectory_msgs::
+ JointTrajectoryPoint::ConstPtr &referencePoint)
{
for(unsigned int i=0;i < qr.rows();i++)
{
}
}
}
-PLUGINLIB_DECLARE_CLASS(wam_controllers,ComputedTorqueController,wam_controllers::ComputedTorqueController,controller_interface::ControllerBase)
+
+PLUGINLIB_DECLARE_CLASS(wam_controllers,ComputedTorqueController,
+ wam_controllers::ComputedTorqueController,controller_interface::ControllerBase)
<library path="lib/libwam_controllers">
- <class name="wam_controllers/ComputedTorqueController" type="wam_controllers::ComputedTorqueController" base_class_type="controller_interface::ControllerBase">
+ <class name="wam_controllers/ComputedTorqueController"
+ type="wam_controllers::ComputedTorqueController"
+ base_class_type="controller_interface::ControllerBase">
<description>
The ComputedTorqueControllers linearizes the Barrett WAM dynamic
- model. The linearized inputs are joint accelerations. It expects a
- EffortJointInterface type of hardware interface.
+ model. The linearized inputs are joint accelerations.
+ It expects an EffortJointInterface type of hardware interface.
</description>
</class>
</description>
<author>Walter Fetter Lages</author>
- <license>GNU</license>
+ <license>GPL</license>
<review status="unreviewed" notes=""/>
<url>http://ros.org/wiki/wam_description</url>
<?xml version="1.0"?>
-<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
- xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
- xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
- xmlns:xacro="http://ros.org/wiki/xacro"
- name="wam">
+<robot name="wam"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:xacro="http://ros.org/wiki/xacro">
- <!-- LINKS -->
<include filename="$(find wam_description)/xacro/wam_base.urdf.xacro" />
<include filename="$(find wam_description)/xacro/wam_j1.urdf.xacro" />
<include filename="$(find wam_description)/xacro/wam_j2.urdf.xacro" />
<controlPeriod>0.001</controlPeriod>
</controller:ros_control_gazebo_plugin>
</gazebo>
-
</robot>
-
<child link="wam_link_4"/>
<origin rpy="-1.57079632679 0 0" xyz="0.045 0.0 0.55"/>
<axis xyz="0 0 1"/>
- <limit effort="35" lower="-0.9" upper="2.8" velocity="2.0"/>
+ <limit effort="35" lower="-0.9" upper="3.1" velocity="2.0"/>
<joint_properties damping="100.0" friction="1000.0" />
<dynamics damping="1000"/>
</joint>
projects / ufrgs_wam.git / commitdiff