From cd3d3f5e7bfb1237530f39a79672dcb5ca36a51c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Tue, 16 May 2017 00:33:27 +0200
Subject: [PATCH] Solved a problem with the trajectory speed assignation to
 each joint.

---
 humanoid_common/package.xml                 |  1 +
 humanoid_modules/src/joints_cart_module.cpp | 12 +++++++-----
 2 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/humanoid_common/package.xml b/humanoid_common/package.xml
index e5ca806..7c5140f 100755
--- a/humanoid_common/package.xml
+++ b/humanoid_common/package.xml
@@ -50,6 +50,7 @@
   <run_depend>qr_global_loc</run_depend>
   <run_depend>teleop</run_depend>
   <run_depend>humanoid_modules</run_depend>
+  <run_depend>joints_cart_client</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/humanoid_modules/src/joints_cart_module.cpp b/humanoid_modules/src/joints_cart_module.cpp
index eb4e0d9..d18fa24 100644
--- a/humanoid_modules/src/joints_cart_module.cpp
+++ b/humanoid_modules/src/joints_cart_module.cpp
@@ -184,17 +184,19 @@ void CJointsCartModule::initialize_get_ik_msg(void)
 
 void CJointsCartModule::initialize_action_msg(sensor_msgs::JointState &solution)
 {
-  std::vector<double> speeds;
   unsigned int i=0;
 
   this->joints_traj_msg.trajectory.joint_names=solution.name;
   this->joints_traj_msg.trajectory.points[0].positions=solution.position;
-  if(this->motion_time=-1.0)
-    this->joints_traj_msg.trajectory.points[0].velocities=std::vector<double>(this->joints_traj_msg.trajectory.joint_names.size(),this->config.default_speed);
+  this->joints_traj_msg.trajectory.points[0].velocities.resize(this->joints_traj_msg.trajectory.joint_names.size());
+  if(this->motion_time==-1.0)
+  { 
+    for(i=0;i<this->joints_traj_msg.trajectory.joint_names.size();i++)
+      this->joints_traj_msg.trajectory.points[0].velocities[i]=0.1;//this->config.default_speed;
+  }
   else
   {
-    this->joints_traj_msg.trajectory.points[0].velocities.resize(this->joints_traj_msg.trajectory.joint_names.size());
-    for(i=0;i<speeds.size();i++)
+    for(i=0;i<this->joints_traj_msg.trajectory.joint_names.size();i++)
       this->joints_traj_msg.trajectory.points[0].velocities[i]=this->joints_traj_msg.trajectory.points[0].positions[i]/this->motion_time;
   }
   this->joints_traj_msg.trajectory.points[0].accelerations=std::vector<double>(this->joints_traj_msg.trajectory.joint_names.size(),0.0);
-- 
GitLab