Skip to content

Commit

Permalink
Bug fixes and updates to tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@URJala
URJala committed Jul 10, 2024
1 parent e1797f3 commit 535b083
Show file tree
Hide file tree
Showing 4 changed files with 60 additions and 26 deletions.
27 changes: 19 additions & 8 deletions examples/force_mode_example.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,14 +149,25 @@ int main(int argc, char* argv[])
}
// Task frame at the robot's base with limits being large enough to cover the whole workspace
// Compliance in z axis and rotation around z axis

bool success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base
{ 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis
{ 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench
2, // do not transform the force frame at all
{ 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 } // limits. See ScriptManual
,
0.8, 0.8); // for details.
bool success;
if (g_my_driver->getVersion().major < 5)
success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base
{ 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis
{ 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench
2, // do not transform the force frame at all
{ 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 } // limits. See ScriptManual
,
0.8); // for details.
else
{
success = g_my_driver->startForceMode({ 0, 0, 0, 0, 0, 0 }, // Task frame at the robot's base
{ 0, 0, 1, 0, 0, 1 }, // Compliance in z axis and rotation around z axis
{ 0, 0, 0, 0, 0, 0 }, // do not apply any active wrench
2, // do not transform the force frame at all
{ 0.1, 0.1, 1.5, 3.14, 3.14, 0.5 } // limits. See ScriptManual
,
0.8, 0.8); // for details.
}
if (!success)
{
URCL_LOG_ERROR("Failed to start force mode.");
Expand Down
34 changes: 22 additions & 12 deletions include/ur_client_library/ur/ur_driver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,15 +128,25 @@ class UrDriver
* \param force_mode_damping The damping parameter used when the robot is in force mode, range [0,1]
* \param force_mode_gain_scaling Scales the gain used when the robot is in force mode, range [0,2] (only e-series)
*/
[[deprecated("Specifying the force mode damping factor and the force mode gain scaling factor at driver creation has "
"been deprecated. Force mode parameters should be specified with each activiation of force mode, and "
"can be set in the function call to start force mode.")]]
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
std::unique_ptr<ToolCommSetup> tool_comm_setup, const uint32_t reverse_port,
const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read,
const std::string& reverse_ip, const uint32_t trajectory_port, const uint32_t script_command_port,
double force_mode_damping, double force_mode_gain_scaling = 0.5);
[[deprecated(
"Specifying the force mode damping factor and the force mode gain scaling factor at driver creation has "
"been deprecated. Force mode parameters should be specified with each activiation of force mode, and "
"can be set in the function call to start force mode.")]] UrDriver(const std::string& robot_ip,
const std::string& script_file,
const std::string& output_recipe_file,
const std::string& input_recipe_file,
std::function<void(bool)> handle_program_state,
bool headless_mode,
std::unique_ptr<ToolCommSetup> tool_comm_setup,
const uint32_t reverse_port,
const uint32_t script_sender_port,
int servoj_gain, double servoj_lookahead_time,
bool non_blocking_read,
const std::string& reverse_ip,
const uint32_t trajectory_port,
const uint32_t script_command_port,
double force_mode_damping,
double force_mode_gain_scaling = 0.5);

/*!
* \brief Constructs a new UrDriver object.
Expand Down Expand Up @@ -439,9 +449,9 @@ class UrDriver
* \returns True, if the write was performed successfully, false otherwise.
*/
[[deprecated("Starting force mode without specifying the force mode damping factor and gain scale factor has been "
"deprecated. These values should be given with each function call.")]]
bool startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, const vector6d_t& wrench,
const unsigned int type, const vector6d_t& limits);
"deprecated. These values should be given with each function call.")]] bool
startForceMode(const vector6d_t& task_frame, const vector6uint32_t& selection_vector, const vector6d_t& wrench,
const unsigned int type, const vector6d_t& limits);

/*!
* \brief Stop force mode and put the robot into normal operation mode.
Expand Down
4 changes: 3 additions & 1 deletion src/ur/ur_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -142,7 +142,9 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_
}
begin_replace << "set_tool_voltage("
<< static_cast<std::underlying_type<ToolVoltage>::type>(tool_comm_setup->getToolVoltage()) << ")\n";
begin_replace << "set_tool_communication(" << "True" << ", " << tool_comm_setup->getBaudRate() << ", "
begin_replace << "set_tool_communication("
<< "True"
<< ", " << tool_comm_setup->getBaudRate() << ", "
<< static_cast<std::underlying_type<Parity>::type>(tool_comm_setup->getParity()) << ", "
<< tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", "
<< tool_comm_setup->getTxIdleChars() << ")";
Expand Down
21 changes: 16 additions & 5 deletions tests/test_script_command_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,11 +64,11 @@ class ScriptCommandInterfaceTest : public ::testing::Test

void readMessage(int32_t& command, std::vector<int32_t>& message)
{
// Max message length is 26
uint8_t buf[sizeof(int32_t) * 26];
// Max message length is 28
uint8_t buf[sizeof(int32_t) * 28];
uint8_t* b_pos = buf;
size_t read = 0;
size_t remainder = sizeof(int32_t) * 26;
size_t remainder = sizeof(int32_t) * 28;
while (remainder > 0)
{
if (!TCPSocket::read(b_pos, remainder, read))
Expand Down Expand Up @@ -252,7 +252,10 @@ TEST_F(ScriptCommandInterfaceTest, test_force_mode)
urcl::vector6d_t wrench = { 20, 0, 40, 0, 0, 0 };
int32_t force_mode_type = 2;
urcl::vector6d_t limits = { 0.1, 0.1, 0.1, 0.785, 0.785, 1.57 };
script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, force_mode_type, &limits);
double damping = 0.8;
double gain_scaling = 0.8;
script_command_interface_->startForceMode(&task_frame, &selection_vector, &wrench, force_mode_type, &limits, damping,
gain_scaling);

int32_t command;
std::vector<int32_t> message;
Expand Down Expand Up @@ -298,8 +301,16 @@ TEST_F(ScriptCommandInterfaceTest, test_force_mode)
EXPECT_EQ(received_limits[i], limits[i]);
}

// Test damping return
double received_damping = (double)message[25] / script_command_interface_->MULT_JOINTSTATE;
EXPECT_EQ(received_damping, damping);

// Test Gain scaling return
double received_gain = (double)message[26] / script_command_interface_->MULT_JOINTSTATE;
EXPECT_EQ(received_gain, gain_scaling);

// The rest of the message should be zero
int32_t message_sum = std::accumulate(std::begin(message) + 25, std::end(message), 0);
int32_t message_sum = std::accumulate(std::begin(message) + 27, std::end(message), 0);
int32_t expected_message_sum = 0;
EXPECT_EQ(message_sum, expected_message_sum);

Expand Down

0 comments on commit 535b083

Please sign in to comment.