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