diff --git a/.gitignore b/.gitignore new file mode 100755 index 0000000..dbe9c82 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode/ \ No newline at end of file diff --git a/CHANGELOG.rst b/CHANGELOG.rst old mode 100644 new mode 100755 diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/README.rst b/README.rst old mode 100644 new mode 100755 diff --git a/include/naoqi_dcm_driver/dcm.hpp b/include/naoqi_dcm_driver/dcm.hpp old mode 100644 new mode 100755 diff --git a/include/naoqi_dcm_driver/diagnostics.hpp b/include/naoqi_dcm_driver/diagnostics.hpp old mode 100644 new mode 100755 diff --git a/include/naoqi_dcm_driver/memory.hpp b/include/naoqi_dcm_driver/memory.hpp old mode 100644 new mode 100755 diff --git a/include/naoqi_dcm_driver/motion.hpp b/include/naoqi_dcm_driver/motion.hpp old mode 100644 new mode 100755 diff --git a/include/naoqi_dcm_driver/robot.hpp b/include/naoqi_dcm_driver/robot.hpp old mode 100644 new mode 100755 index 292487d..27c7086 --- a/include/naoqi_dcm_driver/robot.hpp +++ b/include/naoqi_dcm_driver/robot.hpp @@ -198,6 +198,9 @@ class Robot : public hardware_interface::RobotHW /** joints positions from ROS hardware interface */ hardware_interface::PositionJointInterface jnt_pos_interface_; + /** joints efforts from ROS hardware interface */ // Mainly to set stiffness + hardware_interface::EffortJointInterface jnt_eff_interface_; + /** Naoqi joints names */ std::vector qi_joints_; diff --git a/include/naoqi_dcm_driver/tools.hpp b/include/naoqi_dcm_driver/tools.hpp old mode 100644 new mode 100755 diff --git a/package.xml b/package.xml old mode 100644 new mode 100755 diff --git a/src/dcm.cpp b/src/dcm.cpp old mode 100644 new mode 100755 diff --git a/src/diagnostics.cpp b/src/diagnostics.cpp old mode 100644 new mode 100755 diff --git a/src/memory.cpp b/src/memory.cpp old mode 100644 new mode 100755 diff --git a/src/motion.cpp b/src/motion.cpp old mode 100644 new mode 100755 index 3efaca4..76c14b8 --- a/src/motion.cpp +++ b/src/motion.cpp @@ -199,15 +199,9 @@ std::vector Motion::getAngles(const std::string &robot_part) void Motion::writeJoints(const std::vector &joint_commands) { - //prepare the list of joints - qi::AnyValue names_qi = fromStringVectorToAnyValue(joints_names_); - - //prepare the list of joint angles - qi::AnyValue angles_qi = fromDoubleVectorToAnyValue(joint_commands); - try { - motion_proxy_.async("setAngles", names_qi, angles_qi, 0.2f); + motion_proxy_.async("setAngles", joints_names_, joint_commands, 0.2f); } catch(const std::exception& e) { @@ -240,8 +234,6 @@ bool Motion::stiffnessInterpolation(const std::string &motor_group, return false; } - ROS_INFO_STREAM("Stiffness is updated to " << stiffness << " for " << motor_group); - return true; } diff --git a/src/robot.cpp b/src/robot.cpp old mode 100644 new mode 100755 index 1039cb8..29629cb --- a/src/robot.cpp +++ b/src/robot.cpp @@ -108,10 +108,15 @@ bool Robot::initializeControllers(const std::vector &joints) hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(joints.at(i)), &hw_commands_[i]); jnt_pos_interface_.registerHandle(pos_handle); + hw_efforts_[i] = stiffness_value_; + hardware_interface::JointHandle eff_handle(jnt_state_interface_.getHandle(joints.at(i)), + &hw_efforts_[i]); + jnt_eff_interface_.registerHandle(eff_handle); } registerInterface(&jnt_state_interface_); registerInterface(&jnt_pos_interface_); + registerInterface(&jnt_eff_interface_); } catch(const ros::Exception& e) { @@ -330,7 +335,7 @@ void Robot::controllerLoop() readJoints(); //motion_->stiffnessInterpolation(diagnostics_->getForcedJoints(), 0.3f, 2.0f); - + try { manager_->update(time, ros::Duration(1.0f/controller_freq_)); @@ -344,8 +349,8 @@ void Robot::controllerLoop() writeJoints(); //no need if Naoqi Driver is running - //publishJointStateFromAlMotion(); - + publishJointStateFromAlMotion(); + rate.sleep(); } ROS_INFO_STREAM("Shutting down the main loop"); @@ -442,14 +447,16 @@ void Robot::readJoints() //store joints angles std::vector::iterator hw_command_j = hw_commands_.begin(); std::vector::iterator hw_angle_j = hw_angles_.begin(); + std::vector::iterator hw_velocity_j = hw_velocities_.begin(); std::vector::iterator qi_position_j = qi_joints_positions.begin(); std::vector::iterator hw_enabled_j = hw_enabled_.begin(); - for(; hw_command_j != hw_commands_.end(); ++hw_command_j, ++hw_angle_j, ++hw_enabled_j) + for(; hw_command_j != hw_commands_.end(); ++hw_command_j, ++hw_angle_j, ++hw_enabled_j, ++hw_velocity_j) { if (!*hw_enabled_j) continue; + *hw_velocity_j = (*qi_position_j - *hw_angle_j)*controller_freq_; *hw_angle_j = *qi_position_j; // Set commands to the read angles for when no command specified *hw_command_j = *qi_position_j; @@ -461,11 +468,7 @@ void Robot::readJoints() void Robot::publishJointStateFromAlMotion(){ joint_states_topic_.header.stamp = ros::Time::now(); - - std::vector position_data = motion_->getAngles("Body"); - for(int i = 0; igetAngles("Body"); joint_states_pub_.publish(joint_states_topic_); } @@ -473,6 +476,7 @@ void Robot::writeJoints() { // Check if there is some change in joints values bool changed(false); + motion_->stiffnessInterpolation(motor_groups_, (hw_efforts_[0]>1?1:hw_efforts_[0]), 0.001f); std::vector::iterator hw_angle_j = hw_angles_.begin(); std::vector::iterator hw_command_j = hw_commands_.begin(); std::vector::iterator qi_command_j = qi_commands_.begin(); @@ -492,7 +496,7 @@ void Robot::writeJoints() changed = true; } } - + // Update joints values if there are some changes if(!changed) return; diff --git a/src/robot_driver.cpp b/src/robot_driver.cpp old mode 100644 new mode 100755 index fb23a59..d3cba99 --- a/src/robot_driver.cpp +++ b/src/robot_driver.cpp @@ -96,7 +96,7 @@ int main(int argc, char** argv) return -1; } - if (!session->connected) + if (!session->isConnected()) { ROS_ERROR("Cannot connect to session"); session->close(); diff --git a/src/tools.cpp b/src/tools.cpp old mode 100644 new mode 100755