From 3adb74303151459eefb9ee6e167824bb412fed07 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <olivier.stasse@gmail.com>
Date: Thu, 11 Jul 2013 07:06:38 +0200
Subject: [PATCH] Handle parallel joints.
---
src/sot_loader.cpp | 60 +++++++++++++++++++++++++++++++++++++++++++---
src/sot_loader.hh | 11 ++++++++-
2 files changed, 67 insertions(+), 4 deletions(-)
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 489e45f..60b18e5 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -58,6 +58,8 @@ int SotLoader::initPublication()
int SotLoader::readSotVectorStateParam()
{
+ std::map<std::string,int> from_state_name_to_state_vector;
+ std::map<std::string,std::string> from_parallel_name_to_state_vector_name;
ros::NodeHandle n;
if (!ros::param::has("/sot/state_vector_map"))
@@ -70,14 +72,61 @@ int SotLoader::readSotVectorStateParam()
ROS_ASSERT(stateVectorMap_.getType() == XmlRpc::XmlRpcValue::TypeArray);
nbOfJoints_ = stateVectorMap_.size();
+ if (ros::param::has("/sot/joint_state_parallel"))
+ {
+ XmlRpc::XmlRpcValue joint_state_parallel;
+ n.getParam("/sot/joint_state_parallel", joint_state_parallel);
+ ROS_ASSERT(joint_state_parallel.getType() == XmlRpc::XmlRpcValue::TypeStruct);
+ std::cout << "Type of joint_state_parallel:" << joint_state_parallel.getType() << std::endl;
+
+ for(XmlRpc::XmlRpcValue::iterator it = joint_state_parallel.begin();
+ it!= joint_state_parallel.end(); it++)
+ {
+ XmlRpc::XmlRpcValue local_value=it->second;
+ std::string final_expression, map_expression = static_cast<string>(local_value);
+ double final_coefficient = 1.0;
+ // deal with sign
+ if (map_expression[0]=='-')
+ {
+ final_expression = map_expression.substr(1,map_expression.size()-1);
+ final_coefficient = -1.0;
+ }
+ else
+ final_expression = map_expression;
+
+ std::cout <<it->first.c_str() << ": " << final_coefficient << std::endl;
+ from_parallel_name_to_state_vector_name[it->first.c_str()] = final_expression;
+ coefficient_parallel_joints_.push_back(final_coefficient);
+
+ }
+ nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
+ }
+
// Prepare joint_state according to robot description.
- joint_state_.name.resize(nbOfJoints_);
- joint_state_.position.resize(nbOfJoints_);
+ joint_state_.name.resize(nbOfJoints_+nbOfParallelJoints_);
+ joint_state_.position.resize(nbOfJoints_+nbOfParallelJoints_);
+ // Fill in the name of the joints from the state vector.
+ // and build local map from state name to state vector
for (int32_t i = 0; i < stateVectorMap_.size(); ++i)
{
joint_state_.name[i]= static_cast<string>(stateVectorMap_[i]);
+
+ from_state_name_to_state_vector[joint_state_.name[i]] = i;
}
+
+ // Fill in the name of the joints from the parallel joint vector.
+ // and build map from parallel name to state vector
+ int i=0;
+ parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
+ for (std::map<std::string,std::string>::iterator it = from_parallel_name_to_state_vector_name.begin();
+ it != from_parallel_name_to_state_vector_name.end();
+ it++,i++)
+ {
+ joint_state_.name[i+nbOfJoints_]=it->first.c_str();
+ parallel_joints_to_state_vector_[i] = from_state_name_to_state_vector[it->second];
+ }
+
angleEncoder_.resize(nbOfJoints_);
return 0;
@@ -174,7 +223,12 @@ SotLoader::readControl(map<string,dgs::ControlValues> &controlValues)
{
joint_state_.position[i] = angleControl_[i];
}
-
+ for(unsigned int i=0;i<parallel_joints_to_state_vector_.size();i++)
+ {
+ joint_state_.position[i+nbOfJoints_] =
+ coefficient_parallel_joints_[i]*parallel_joints_to_state_vector_[i];
+ }
+
joint_pub_.publish(joint_state_);
diff --git a/src/sot_loader.hh b/src/sot_loader.hh
index ceb5fd2..2d02679 100644
--- a/src/sot_loader.hh
+++ b/src/sot_loader.hh
@@ -62,6 +62,7 @@ protected:
std::map <std::string,dgs::SensorValues> sensorsIn_;
/// Map of control values
std::map<std::string,dgs::ControlValues> controlValues_;
+
/// Angular values read by encoders
std::vector <double> angleEncoder_;
/// Angular values sent to motors
@@ -79,9 +80,16 @@ protected:
/// URDF string description of the robot.
std::string robot_desc_string_;
-
+
+ /// \brief Map between SoT state vector and some joint_state_links
XmlRpc::XmlRpcValue stateVectorMap_;
+ /// \brief List of parallel joints from the state vector.
+ std::vector<int> parallel_joints_to_state_vector_;
+
+ /// \brief Coefficient between parallel joints and the state vector.
+ std::vector<double> coefficient_parallel_joints_;
+
// Joint state publication.
ros::Publisher joint_pub_;
@@ -90,6 +98,7 @@ protected:
// Number of DOFs according to KDL.
int nbOfJoints_;
+ int nbOfParallelJoints_;
public:
--
GitLab