From: Walter Fetter Lages Date: Wed, 28 Nov 2018 13:47:04 +0000 (-0200) Subject: Change upper limit for joint 4. X-Git-Tag: rosbook2015initial~6 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=ee9adbb032dceea34b098b3a980b263183f10799;p=ufrgs_wam.git Change upper limit for joint 4. Change license to GPL. Add scripts to set initial conditions. Add scripts to move the robot. --- diff --git a/stack.xml b/stack.xml index c3783ed..a22a793 100644 --- a/stack.xml +++ b/stack.xml @@ -1,9 +1,8 @@ UFRGS WAM Maintained by Walter Fetter Lages - GNU + GPL http://ros.org/wiki/ufrgs_wam - diff --git a/wam_control_gazebo/manifest.xml b/wam_control_gazebo/manifest.xml index b8569ea..8aa7c58 100644 --- a/wam_control_gazebo/manifest.xml +++ b/wam_control_gazebo/manifest.xml @@ -11,13 +11,10 @@ - - - diff --git a/wam_control_gazebo/robot_sim_plugins.xml b/wam_control_gazebo/robot_sim_plugins.xml index d52e012..94e66ad 100644 --- a/wam_control_gazebo/robot_sim_plugins.xml +++ b/wam_control_gazebo/robot_sim_plugins.xml @@ -5,8 +5,9 @@ type="wam_control_gazebo::RobotSimWam" base_class_type="ros_control_gazebo::RobotSim"> - 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. + diff --git a/wam_control_gazebo/src/robot_sim_wam.cpp b/wam_control_gazebo/src/robot_sim_wam.cpp index 5eead8e..203216c 100644 --- a/wam_control_gazebo/src/robot_sim_wam.cpp +++ b/wam_control_gazebo/src/robot_sim_wam.cpp @@ -13,10 +13,8 @@ namespace wam_control_gazebo { - class RobotSimWam:public ros_control_gazebo::RobotSim { - unsigned int n_dof_; hardware_interface::JointStateInterface js_interface_; @@ -35,7 +33,6 @@ namespace wam_control_gazebo 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"; @@ -49,11 +46,12 @@ namespace wam_control_gazebo 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_); @@ -82,21 +80,20 @@ namespace wam_control_gazebo { 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) diff --git a/wam_controllers/include/wam_controllers/computed_torque_controller.h b/wam_controllers/include/wam_controllers/computed_torque_controller.h index 60a09ea..825dac1 100644 --- a/wam_controllers/include/wam_controllers/computed_torque_controller.h +++ b/wam_controllers/include/wam_controllers/computed_torque_controller.h @@ -17,45 +17,46 @@ #include #include - namespace wam_controllers { - class ComputedTorqueController: public controller_interface::Controller + 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_; + ros::NodeHandle node_; + + hardware_interface::EffortJointInterface *robot_; + std::vector 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 diff --git a/wam_controllers/launch/computed_torque.launch b/wam_controllers/launch/computed_torque.launch index 2da9e12..40d04a1 100644 --- a/wam_controllers/launch/computed_torque.launch +++ b/wam_controllers/launch/computed_torque.launch @@ -5,10 +5,13 @@ - + - + - + diff --git a/wam_controllers/manifest.xml b/wam_controllers/manifest.xml index 2fd288c..8f8c463 100644 --- a/wam_controllers/manifest.xml +++ b/wam_controllers/manifest.xml @@ -8,8 +8,8 @@ GPL http://ros.org/wiki/wam_controllers + - @@ -21,5 +21,3 @@ - - diff --git a/wam_controllers/scripts/move_zero.sh b/wam_controllers/scripts/move_zero.sh new file mode 100755 index 0000000..ec582ab --- /dev/null +++ b/wam_controllers/scripts/move_zero.sh @@ -0,0 +1,7 @@ +#!/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 diff --git a/wam_controllers/scripts/set_home.sh b/wam_controllers/scripts/set_home.sh new file mode 100755 index 0000000..62f7e9c --- /dev/null +++ b/wam_controllers/scripts/set_home.sh @@ -0,0 +1,12 @@ +#!/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 diff --git a/wam_controllers/scripts/set_initial.sh b/wam_controllers/scripts/set_initial.sh new file mode 100755 index 0000000..d1099b3 --- /dev/null +++ b/wam_controllers/scripts/set_initial.sh @@ -0,0 +1,12 @@ +#!/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 diff --git a/wam_controllers/src/computed_torque_controller.cpp b/wam_controllers/src/computed_torque_controller.cpp index 407a982..3c82b8e 100644 --- a/wam_controllers/src/computed_torque_controller.cpp +++ b/wam_controllers/src/computed_torque_controller.cpp @@ -9,11 +9,10 @@ #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) { } @@ -22,7 +21,8 @@ namespace wam_controllers 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; @@ -30,13 +30,15 @@ namespace wam_controllers 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; } @@ -45,14 +47,17 @@ namespace wam_controllers 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)) @@ -75,7 +80,6 @@ namespace wam_controllers 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); @@ -87,10 +91,9 @@ namespace wam_controllers 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()); @@ -106,15 +109,14 @@ namespace wam_controllers 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; @@ -123,44 +125,23 @@ namespace wam_controllers 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++) { @@ -170,4 +151,6 @@ namespace wam_controllers } } } -PLUGINLIB_DECLARE_CLASS(wam_controllers,ComputedTorqueController,wam_controllers::ComputedTorqueController,controller_interface::ControllerBase) + +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 index 4be4031..55b3dd3 100644 --- a/wam_controllers/wam_controllers_plugins.xml +++ b/wam_controllers/wam_controllers_plugins.xml @@ -1,10 +1,12 @@ - + 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. diff --git a/wam_description/manifest.xml b/wam_description/manifest.xml index 44032f1..123238f 100644 --- a/wam_description/manifest.xml +++ b/wam_description/manifest.xml @@ -5,7 +5,7 @@ Walter Fetter Lages - GNU + GPL http://ros.org/wiki/wam_description diff --git a/wam_description/xacro/wam.urdf.xacro b/wam_description/xacro/wam.urdf.xacro index 6fae73e..8a70fd3 100644 --- a/wam_description/xacro/wam.urdf.xacro +++ b/wam_description/xacro/wam.urdf.xacro @@ -1,11 +1,10 @@ - + - @@ -45,6 +44,4 @@ 0.001 - - diff --git a/wam_description/xacro/wam_j4.urdf.xacro b/wam_description/xacro/wam_j4.urdf.xacro index c357a29..3a8c5ec 100644 --- a/wam_description/xacro/wam_j4.urdf.xacro +++ b/wam_description/xacro/wam_j4.urdf.xacro @@ -38,7 +38,7 @@ - +