From 12b92c14db9a19f7ed0c7d002418f7948656a036 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Wed, 16 Sep 2015 15:08:46 +0200 Subject: [PATCH] Bugfixes in URScript commands --- src/ur_driver.cpp | 12 ++++++------ src/ur_realtime_communication.cpp | 2 +- src/ur_ros_wrapper.cpp | 1 - 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 06b553b..000a1d4 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -86,7 +86,7 @@ void UrDriver::addTraj(std::vector inp_timestamps, timestamps[i] - timestamps[i - 1]); command_string += buf; } - command_string += "end\ntraj()\n"; + command_string += "end\n"; //printf("%s", command_string.c_str()); rt_interface_->addCommandToQueue(command_string); @@ -123,14 +123,14 @@ void UrDriver::setJointNames(std::vector jn) { void UrDriver::setToolVoltage(unsigned int v) { char buf[256]; sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); - printf("%s", buf); + //printf("%s", buf); rt_interface_->addCommandToQueue(buf); } void UrDriver::setFlag(unsigned int n, bool b) { char buf[256]; sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, b ? "True" : "False"); - printf("%s", buf); + //printf("%s", buf); rt_interface_->addCommandToQueue(buf); } @@ -138,14 +138,14 @@ void UrDriver::setDigitalOut(unsigned int n, bool b) { char buf[256]; sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, b ? "True" : "False"); - printf("%s", buf); + //printf("%s", buf); rt_interface_->addCommandToQueue(buf); } void UrDriver::setAnalogOut(unsigned int n, double f) { char buf[256]; - sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d, %1.4f)\nend\n", n, f); - printf("%s", buf); + sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); + //printf("%s", buf); rt_interface_->addCommandToQueue(buf); } diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 8e66b4d..a7cb464 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -83,7 +83,7 @@ void UrRealtimeCommunication::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc) { char cmd[1024]; sprintf(cmd, - "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %1.5f,0.02)\n", + "speedj([%1.5f, %1.5f, %1.5f, %1.5f, %1.5f, %1.5f], %f, 0.02)\n", q0, q1, q2, q3, q4, q5, acc); addCommandToQueue((std::string) (cmd)); if (q0 != 0. or q1 != 0. or q2 != 0. or q3 != 0. or q4 != 0. or q5 != 0.) { diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 0079f76..b0b5a1e 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -317,7 +317,6 @@ private: } void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) { - reorder_traj_joints(*msg); double acc = *std::max_element(msg->points[0].accelerations.begin(), msg->points[0].accelerations.end()); robot_.setSpeed(msg->points[0].velocities[0],