trajectory_msgs
wam_msgs
wam_srvs
+ eigen_conversions
)
find_package(cmake_modules REQUIRED)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES wam_node_sim
- CATKIN_DEPENDS geometry_msgs kdl_parser roscpp std_msgs std_srvs trajectory_msgs wam_msgs wam_srvs
+ CATKIN_DEPENDS geometry_msgs kdl_parser roscpp std_msgs std_srvs trajectory_msgs wam_msgs wam_srvs eigen_conversions
# DEPENDS Eigen3
)
#include <Eigen/Core>
//#include <Eigen/Geometry>
-//#include <libconfig.h>
+#include <eigen_conversions/eigen_kdl.h>
#include <kdl/frames.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/velocityprofile_trap.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainiksolverpos_lma.hpp>
-//#include <wam_node_sim/rate_limiter.h>
+
+#include <wam_node_sim/rate_limiter.h>
namespace barrett
{
if (!kdl_parser::treeFromString(robotDescription,tree_))
ROS_ERROR("Failed to construct KDL tree.");
-
KDL::SegmentMap::const_iterator s=tree_.getRootSegment();
if (!tree_.getChain(GetTreeElementSegment(s->second).getName(),"wam_tool_plate",chain_))
ROS_ERROR("Failed to get chain from KDL tree.");
- fwdKinSolverPos_=new KDL::ChainFkSolverPos_recursive(chain_);
+ fkSolverPos_=new KDL::ChainFkSolverPos_recursive(chain_);
+ Eigen::Matrix<double,6,1> L;
+ L << 1.0, 1.0, 1.0, 0.0, 0.0, 0.0;
+ ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain_,L);
jointPosition_.setZero();
jointVelocity_.setZero();
{
wam_cmd_pub.shutdown();
jointStatesSubscriber_.shutdown();
- delete fwdKinSolverPos_;
+ delete fkSolverPos_;
+ delete ikSolverPos_;
}
template<size_t DOF>
}
template<size_t DOF>
+void Wam<DOF>::trackReferenceSignal(cp_type &referenceSignal)
+{
+ KDL::JntArray q0(DOF);
+ q0.data=jointPosition_;
+ // try to induce convergence to an out-elbow pose
+ q0(3)=3.1;
+ q0(4)=0.0;
+ q0(5)=0.0;
+ q0(6)=0.0;
+ KDL::Frame frame(KDL::Vector(referenceSignal[0],referenceSignal[1],referenceSignal[2]));
+ KDL::JntArray q(DOF);
+ int error;
+ if((error=ikSolverPos_->CartToJnt(q0,frame,q)) < 0)
+ ROS_ERROR_STREAM("Failed to compute inverse kinematics: " << ikSolverPos_->strError(error));
+ jp_type jp;
+ jp=q.data;
+ trackReferenceSignal(jp);
+}
+
+template<size_t DOF>
inline const typename Wam<DOF>::jp_type& Wam<DOF>::getHomePosition() const
{
static jp_type home={0.0, -2.0, 0.0, 3.1, 0.0, 0.0, 0.0};
return home;
}
-
template<size_t DOF>
typename Wam<DOF>::jt_type Wam<DOF>::getJointTorques() const
{
{
ros::spinOnce();
KDL::JntArray jointPosition(DOF);
- for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i];
+ for(int i=0;i < DOF;i++) jointPosition(i)=jointPosition_[i];
KDL::Frame frame;
- if(fwdKinSolverPos_->JntToCart(jointPosition,frame) < 0)
- ROS_ERROR("Failed to compute forward kinematics.");
+ int error;
+ if((error=fkSolverPos_->JntToCart(jointPosition,frame)) < 0)
+ ROS_ERROR_STREAM("Failed to compute forward kinematics: " << fkSolverPos_->strError(error));
cp_type cp;
- for(int i=0;i < 3;i++) cp[i]=frame.p.data[i];
+ tf::vectorKDLToEigen(frame.p,cp);
return cp;
}
{
ros::spinOnce();
KDL::JntArray jointPosition(DOF);
- for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i];
+ for(int i=0;i < DOF;i++) jointPosition(i)=jointPosition_[i];
KDL::Frame frame;
- if(fwdKinSolverPos_->JntToCart(jointPosition,frame) < 0)
- ROS_ERROR("Failed to compute forward kinematics.");
+ int error;
+ if((error=fkSolverPos_->JntToCart(jointPosition,frame)) < 0)
+ ROS_ERROR_STREAM("Failed to compute forward kinematics: " << fkSolverPos_->strError(error));
double x,y,z,w;
frame.M.GetQuaternion(x,y,z,w);
template<size_t DOF>
inline void Wam<DOF>::moveTo(const cp_type& destination, bool blocking, double velocity, double acceleration)
{
-// moveTo(currentPosHelper(getToolPosition()), /*cv_type(0.0),*/ destination, blocking, velocity, acceleration);
+ moveTo(currentPosHelper(getToolPosition()), /*cv_type(0.0),*/ destination, blocking, velocity, acceleration);
}
template<size_t DOF>
#include <sensor_msgs/JointState.h>
#include <kdl/tree.hpp>
-#include <kdl/chainfksolverpos_recursive.hpp>
+#include <kdl/chainfksolver.hpp>
+#include <kdl/chainiksolver.hpp>
+
#include <wam_node_sim/hand.h>
namespace barrett
/** trackReferenceSignal() used for following updating input. (any barrett input units for control)
*/
void trackReferenceSignal(jp_type & referenceSignal); //NOLINT: non-const reference for syntax
+ void trackReferenceSignal(cp_type & referenceSignal); //NOLINT: non-const reference for syntax
/** getHomePosition() returns home postion of individual joints in Radians
*/
// Used to calculate TP and TO if the values aren't already being calculated in the control loop.
KDL::Tree tree_;
KDL::Chain chain_;
- KDL::ChainFkSolverPos_recursive *fwdKinSolverPos_;
+ KDL::ChainFkSolverPos *fkSolverPos_;
+ KDL::ChainIkSolverPos *ikSolverPos_;
+
ros::Publisher wam_cmd_pub; // controller command
ros::Subscriber jointStatesSubscriber_;
<build_depend>trajectory_msgs</build_depend>
<build_depend>wam_msgs</build_depend>
<build_depend>wam_srvs</build_depend>
+ <build_depend>eigen_conversions</build_depend>
<build_export_depend>Eigen3</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>kdl_parser</build_export_depend>
<build_export_depend>trajectory_msgs</build_export_depend>
<build_export_depend>wam_msgs</build_export_depend>
<build_export_depend>wam_srvs</build_export_depend>
+ <build_export_depend>eigen_conversions</build_export_depend>
<exec_depend>Eigen3</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>kdl_parser</exec_depend>
<exec_depend>trajectory_msgs</exec_depend>
<exec_depend>wam_msgs</exec_depend>
<exec_depend>wam_srvs</exec_depend>
+ <exec_depend>eigen_conversions</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
--- /dev/null
+#!/bin/bash
+
+if [ "$#" -ne 3 ]; then
+ echo "Error: There should be 3 parameters."
+ exit -1;
+fi;
+rosservice call /wam/cart_move "[$1, $2, $3]"
// ortn_vel_sub = n_.subscribe("ortn_vel_cmd", 1, &WamNode::ortnVelCB, this); // wam/ortn_vel_cmd
// jnt_vel_sub = n_.subscribe("jnt_vel_cmd", 1, &WamNode::jntVelCB, this); // wam/jnt_vel_cmd
jnt_pos_sub = n_.subscribe("jnt_pos_cmd", 1, &WamNode::jntPosCB, this); // wam/jnt_pos_cmd
-// cart_pos_sub = n_.subscribe("cart_pos_cmd", 1, &WamNode::cartPosCB, this); // wam/cart_pos_cmd
+ cart_pos_sub = n_.subscribe("cart_pos_cmd", 1, &WamNode::cartPosCB, this); // wam/cart_pos_cmd
//Advertising the following rosservices
hold_ortn_srv = n_.advertiseService("hold_ortn", &WamNode::holdOrtn, this); // wam/hold_ortn
joint_move_srv = n_.advertiseService("joint_move", &WamNode::jointMove, this); // wam/joint_move
pose_move_srv = n_.advertiseService("pose_move", &WamNode::poseMove, this); // wam/pose_move
-// cart_move_srv = n_.advertiseService("cart_move", &WamNode::cartMove, this); // wam/cart_pos_move
+ cart_move_srv = n_.advertiseService("cart_move", &WamNode::cartMove, this); // wam/cart_pos_move
// ortn_move_srv = n_.advertiseService("ortn_move", &WamNode::ortnMove, this); // wam/ortn_move
}
{
if (!cart_pos_status)
{
-/*
- cp_track.setValue(wam.getToolPosition());
- current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation
+ cp_type cp_start = wam.getToolPosition();
+ cp_rl.setCurVal(cp_start); // setting initial the joint position command
cp_rl.setLimit(rt_cp_rl);
- systems::forceConnect(cp_track.output, cp_rl.input);
- systems::forceConnect(cp_rl.output, rt_pose_cmd.getInput<0>()); // saving the rate limited cartesian position command to the pose.position
- systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation
- wam.trackReferenceSignal(rt_pose_cmd.output); //Commanding the WAM to track the real-time pose command.
-*/
+ wam.trackReferenceSignal(cp_rl.getLimit(cp_start)); // command the WAM to track the RT commanded up to (500 Hz) Cartesian positions
}
- else if (new_rt_cmd)
+ else
{
-// cp_track.setValue(rt_cp_cmd); // Set our cartesian positions to subscribed command
-// cp_rl.setLimit(rt_cp_rl); // Updating the rate limit to subscribed rate to control the rate of the moves
+ if(new_rt_cmd) cp_rl.setLimit(rt_cp_rl); // set our rate limit to subscribed rate to control the rate of the moves
+ wam.trackReferenceSignal(cp_rl.getLimit(rt_cp_cmd));
}
cart_pos_status = true;
new_rt_cmd = false;
{
ros::spinOnce();
wam_node.publishWam();
+ ros::spinOnce();
wam_node.updateRT();
+ ros::spinOnce();
loop.sleep();
}
projects / ufrgs_wam.git / commitdiff