From ff319822de0e907bdb1c8df556151dfee1f6a869 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Fri, 30 Jun 2017 12:54:58 +0200
Subject: [PATCH] Used the fabs function to compute the velocity of each joint
(speed must always be positive).
---
humanoid_modules/src/joints_cart_module.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/humanoid_modules/src/joints_cart_module.cpp b/humanoid_modules/src/joints_cart_module.cpp
index ce70eb5..065fb02 100644
--- a/humanoid_modules/src/joints_cart_module.cpp
+++ b/humanoid_modules/src/joints_cart_module.cpp
@@ -215,7 +215,7 @@ void CJointsCartModule::initialize_action_msg(sensor_msgs::JointState &solution)
for(j=0;j<this->current_angles.name.size();j++)
{
if(this->current_angles.name[j]==this->joints_traj_msg.trajectory.joint_names[i])
- this->joints_traj_msg.trajectory.points[0].velocities[i]=(this->current_angles.position[j]-this->joints_traj_msg.trajectory.points[0].positions[i])/this->motion_time;
+ this->joints_traj_msg.trajectory.points[0].velocities[i]=fabs(this->current_angles.position[j]-this->joints_traj_msg.trajectory.points[0].positions[i])/this->motion_time;
}
}
this->joint_state_access.exit();
--
GitLab