Two motors move at the same time #563

Alberto-D · 2022-05-18T15:03:08Z

ISSUE TEMPLATE ver. 1.2.0

Please fill this template for more accurate and prompt support.

Which DYNAMIXEL SDK version do you use?
DynamixelSDK 3.7.40, downloaded from the noetic-devel branch in Github
Which programming language/tool do you use?
ROS Noetic witch c++
Which operating system do you use?
Ubuntu 20.04
Which USB serial converter do you use?
U2D2 from the DYNAMIXEL Starter Set [INT]
Which DYNAMIXEL do you use?
AX-18A
Have you searched the issue from the closed issue threads?
Yes, but i did not found an answer
Please describe the issue in detail
When trying to move two motors using the example code ( https://www.youtube.com/watch?v=SpdxjsCO9sE ) both of them move at the same time, when i connect only one it works fine ( i have to comment the conexion to the second motor to use only one) , but if i connect two (either connecting one motor to the U2D2 and the other motor to the first one, or connecting both motors directly to the U2D2) i get an error: [ERROR] [1652884359.772566946]: Failed to enable torque for Dynamixel ID 1
I guessed this happend because the error codes for the XL430-W250 (motor used in the example) and the AX-18A are different, so i commented:
if (dxl_comm_resultt != COMM_SUCCESS) {
ROS_ERROR("Failed to enable torque for Dynamixel ID %d", DXL2_ID);
return -1;
}

to see if it worked, and i did, but both motors appear to have the same id (1) and so they move at the same time.
I tried to use rosrun dynamixel_workbench_controllers find_dynamixel /dev/ttyUSB0 but it only detects one motor, and only if only one is connected, if i connect both of them, it doesn't detect anything.
Is there a way to control them individually?

How can we reproduce the issue?

ROBOTIS-Will · 2022-05-19T07:53:57Z

Hi @Alberto-D
In order to control multiple DYNAMIXEL, you should assign different ID for each DYNAMIXEL.
Setting both DYNAMIXEL to ID 1 will result in communication failure.

If you use the Broadcast ID 254 instead of each DYNAMIXEL ID, you will be able move multiple AX-18A with different IDs at the same time, but won't be able to receive any status packet from them (such as reading Present Position)

Please see below eManual for more details.
https://emanual.robotis.com/docs/en/dxl/protocol1/#write

Thanks!

Alberto-D · 2022-05-20T08:28:18Z

I assing different ID to each motor with
#define DXL1_ID 1 // DXL1 ID
#define DXL2_ID 2 // DXL2 ID

dxl_comm_result = packetHandler->write1ByteTxRx(
portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error);
dxl_comm_result = packetHandler->write1ByteTxRx(
portHandler, DXL2_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error);

I have commented // if (dxl_comm_result != COMM_SUCCESS) {
// ROS_ERROR("Failed to enable torque for Dynamixel 1 ID %d", DXL1_ID);
// return -1;
// }
both times because if i don't i get an error launching the node.

But when i try to sent messages with rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 100}"
it only works with id:1 and it moves both motors

ROBOTIS-Will · 2022-05-23T02:40:38Z

@Alberto-D
Have you modified the read_write_node.cpp in order to run the Protocol 1.0 DYNAMIXEL?
I didn't have any problem when I tested with the modified code below on my Ubuntu 16.04.

// Copyright 2020 ROBOTIS CO., LTD.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*******************************************************************************
 * This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series with U2D2.
 * For other series, please refer to the product eManual and modify the Control Table addresses and other definitions.
 * To test this example, please follow the commands below.
 *
 * Open terminal #1
 * $ roscore
 *
 * Open terminal #2
 * $ rosrun dynamixel_sdk_examples read_write_node
 *
 * Open terminal #3 (run one of below commands at a time)
 * $ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 0}"
 * $ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 1000}"
 * $ rosservice call /get_position "id: 1"
 * $ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 2, position: 0}"
 * $ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 2, position: 1000}"
 * $ rosservice call /get_position "id: 2"
 *
 * Author: Zerom
*******************************************************************************/

#include <ros/ros.h>

#include "std_msgs/String.h"
#include "dynamixel_sdk_examples/GetPosition.h"
#include "dynamixel_sdk_examples/SetPosition.h"
#include "dynamixel_sdk/dynamixel_sdk.h"

using namespace dynamixel;

// Control table address
#define ADDR_TORQUE_ENABLE    24
#define ADDR_GOAL_POSITION    30
#define ADDR_PRESENT_POSITION 36

// Protocol version
#define PROTOCOL_VERSION      1.0             // Default Protocol version of DYNAMIXEL X series.

// Default setting
#define DXL1_ID               1               // DXL1 ID
#define DXL2_ID               2               // DXL2 ID
#define BAUDRATE              57600           // Default Baudrate of DYNAMIXEL X series
#define DEVICE_NAME           "/dev/ttyUSB0"  // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command

PortHandler * portHandler;
PacketHandler * packetHandler;

bool getPresentPositionCallback(
  dynamixel_sdk_examples::GetPosition::Request & req,
  dynamixel_sdk_examples::GetPosition::Response & res)
{
  uint8_t dxl_error = 0;
  int dxl_comm_result = COMM_TX_FAIL;

  // Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(int16_t) for the Position Value.
  int16_t position = 0;

  // Read Present Position (length : 4 bytes) and Convert uint32 -> int32
  // When reading 2 byte data from AX / MX(1.0), use read2ByteTxRx() instead.
  dxl_comm_result = packetHandler->read2ByteTxRx(
    portHandler, (uint8_t)req.id, ADDR_PRESENT_POSITION, (uint16_t *)&position, &dxl_error);
  if (dxl_comm_result == COMM_SUCCESS) {
    ROS_INFO("getPosition : [ID:%d] -> [POSITION:%d]", req.id, position);
    res.position = position;
    return true;
  } else {
    ROS_INFO("Failed to get position! Result: %d", dxl_comm_result);
    return false;
  }
}

void setPositionCallback(const dynamixel_sdk_examples::SetPosition::ConstPtr & msg)
{
  uint8_t dxl_error = 0;
  int dxl_comm_result = COMM_TX_FAIL;

  // Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value.
  uint16_t position = (unsigned int)msg->position; // Convert int32 -> uint32

  // Write Goal Position (length : 4 bytes)
  // When writing 2 byte data to AX / MX(1.0), use write2ByteTxRx() instead.
  dxl_comm_result = packetHandler->write2ByteTxRx(
    portHandler, (uint8_t)msg->id, ADDR_GOAL_POSITION, position, &dxl_error);
  if (dxl_comm_result == COMM_SUCCESS) {
    ROS_INFO("setPosition : [ID:%d] [POSITION:%d]", msg->id, msg->position);
  } else {
    ROS_ERROR("Failed to set position! Result: %d", dxl_comm_result);
  }
}

int main(int argc, char ** argv)
{
  uint8_t dxl_error = 0;
  int dxl_comm_result = COMM_TX_FAIL;

  portHandler = PortHandler::getPortHandler(DEVICE_NAME);
  packetHandler = PacketHandler::getPacketHandler(PROTOCOL_VERSION);

  if (!portHandler->openPort()) {
    ROS_ERROR("Failed to open the port!");
    return -1;
  }

  if (!portHandler->setBaudRate(BAUDRATE)) {
    ROS_ERROR("Failed to set the baudrate!");
    return -1;
  }

  dxl_comm_result = packetHandler->write1ByteTxRx(
    portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS) {
    ROS_ERROR("Failed to enable torque for Dynamixel ID %d", DXL1_ID);
    return -1;
  }

  dxl_comm_result = packetHandler->write1ByteTxRx(
    portHandler, DXL2_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS) {
    ROS_ERROR("Failed to enable torque for Dynamixel ID %d", DXL2_ID);
    return -1;
  }

  ros::init(argc, argv, "read_write_node");
  ros::NodeHandle nh;
  ros::ServiceServer get_position_srv = nh.advertiseService("/get_position", getPresentPositionCallback);
  ros::Subscriber set_position_sub = nh.subscribe("/set_position", 10, setPositionCallback);
  ros::spin();

  portHandler->closePort();
  return 0;
}

Alberto-D · 2022-05-23T16:02:53Z

Yes, I changer the protocol to 1.0, right now one motor is connected to the other, and one motor to the U2D2, here is my code,

// Copyright 2020 ROBOTIS CO., LTD.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*******************************************************************************

This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series with U2D2.
For other series, please refer to the product eManual and modify the Control Table addresses and other definitions.
To test this example, please follow the commands below.
Open terminal I got this problem in MatLab ...dxl/DXL_SDK_LINUX_v1_01/src/dynamixel.so: clase ELF errónea: ELFCLASS32 #1
$ roscore
Open terminal Different return delay time #2
$ rosrun dynamixel_sdk_examples read_write_node
Open terminal potential out-of-bounds write in Protocol1PacketHandler::WriteTxRx #3 (run one of below commands at a time)
$ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 0}"
$ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 1000}"
$ rosservice call /get_position "id: 1"
$ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 2, position: 0}"
$ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 2, position: 1000}"
$ rosservice call /get_position "id: 2"
Author: Zerom
*******************************************************************************/

#include <ros/ros.h>

#include "std_msgs/String.h"
#include "dynamixel_sdk_examples/GetPosition.h"
#include "dynamixel_sdk_examples/SetPosition.h"
#include "dynamixel_sdk/dynamixel_sdk.h"

using namespace dynamixel;

//For AX-18
// Control table address
#define ADDR_TORQUE_ENABLE 24
#define ADDR_GOAL_POSITION 30
#define ADDR_PRESENT_POSITION 136

// Protocol version
#define PROTOCOL_VERSION 1.0

// Default setting
#define DXL1_ID 1 // DXL1 ID
#define DXL2_ID 2 // DXL2 ID
#define BAUDRATE 1000000

// //For XL-320
// // Control table address
// #define ADDR_TORQUE_ENABLE 24
// #define ADDR_GOAL_POSITION 30
// #define ADDR_PRESENT_POSITION 37

// // Protocol version
// #define PROTOCOL_VERSION 2.0

// // Default setting
// #define DXL1_ID 1 // DXL1 ID
// //#define DXL2_ID 2 // DXL2 ID
// #define BAUDRATE 57600

#define DEVICE_NAME "/dev/ttyUSB0" // [Linux] To find assigned port, use "$ ls /dev/ttyUSB*" command

PortHandler * portHandler;
PacketHandler * packetHandler;

bool getPresentPositionCallback(
dynamixel_sdk_examples::GetPosition::Request & req,
dynamixel_sdk_examples::GetPosition::Response & res)
{
uint8_t dxl_error = 0;
int dxl_comm_result = COMM_TX_FAIL;

// Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(int16_t) for the Position Value.
int32_t position = 0;

// Read Present Position (length : 4 bytes) and Convert uint32 -> int32
// When reading 2 byte data from AX / MX(1.0), use read2ByteTxRx() instead.
dxl_comm_result = packetHandler->read2ByteTxRx(
portHandler, (uint8_t)req.id, ADDR_PRESENT_POSITION, (uint16_t *)&position, &dxl_error);
if (dxl_comm_result == COMM_SUCCESS) {
ROS_INFO("getPosition : [ID:%d] -> [POSITION:%d]", req.id, position);
res.position = position;
return true;
} else {
ROS_INFO("Failed to get position! Result: %d", dxl_comm_result);
return false;
}
}

void setPositionCallback(const dynamixel_sdk_examples::SetPosition::ConstPtr & msg)
{
uint8_t dxl_error = 0;
int dxl_comm_result = COMM_TX_FAIL;

// Position Value of X series is 4 byte data. For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value.
uint16_t position = (unsigned int)msg->position; // Convert int32 -> uint32

// Write Goal Position (length : 4 bytes)
// When writing 2 byte data to AX / MX(1.0), use write2ByteTxRx() instead.
dxl_comm_result = packetHandler->write2ByteTxRx(
portHandler, (uint8_t)msg->id, ADDR_GOAL_POSITION, position, &dxl_error);
if (dxl_comm_result == COMM_SUCCESS) {
ROS_INFO("setPosition : [ID:%d] [POSITION:%d]", msg->id, msg->position);
} else {
ROS_ERROR("Failed to set position! Result: %d", dxl_comm_result);
}
}

int main(int argc, char ** argv)
{
uint8_t dxl_error = 0;
int dxl_comm_result = COMM_TX_FAIL;
int dxl_comm_resultt = COMM_TX_FAIL;

portHandler = PortHandler::getPortHandler(DEVICE_NAME);
packetHandler = PacketHandler::getPacketHandler(PROTOCOL_VERSION);

if (!portHandler->openPort()) {
ROS_ERROR("Failed to open the port!");
return -1;
}

if (!portHandler->setBaudRate(BAUDRATE)) {
ROS_ERROR("Failed to set the baudrate!");
return -1;
}

dxl_comm_result = packetHandler->write1ByteTxRx(
portHandler, DXL1_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error);
// if (dxl_comm_result != COMM_SUCCESS) {
// ROS_ERROR("Failed to enable torque for Dynamixel 1 ID %d", DXL1_ID);
// return -1;
// }
// This is commented to avoid the error at launch

dxl_comm_resultt = packetHandler->write1ByteTxRx(
portHandler, DXL2_ID, ADDR_TORQUE_ENABLE, 1, &dxl_error);
// if (dxl_comm_resultt != COMM_SUCCESS) {
// ROS_ERROR("Failed to enable torque for Dynamixel 2 ID %d", DXL2_ID);
// return -1;
// }

ros::init(argc, argv, "read_write_node");
ros::NodeHandle nh;
ros::ServiceServer get_position_srv = nh.advertiseService("/get_position", getPresentPositionCallback);
ros::Subscriber set_position_sub = nh.subscribe("/set_position", 10, setPositionCallback);
ros::spin();

portHandler->closePort();
return 0;
}

ROBOTIS-Will · 2022-05-23T23:39:14Z

@Alberto-D
Have you tried running the code that I copied?

Alberto-D · 2022-05-24T09:18:45Z

Yes, i get the error [ERROR] [1653382767.128744369]: Failed to enable torque for Dynamixel ID 1 when launching
rosrun dynamixel_sdk_examples read_write_node

if i just copy your code, i guess it is because the error code for the AX series is not the same as for the dlx.

If I comment
if (dxl_comm_result != COMM_SUCCESS) {
ROS_ERROR("Failed to enable torque for Dynamixel ID %d", DXL2_ID);
return -1;
}
both times I get [ERROR] [1653382927.851254788]: Failed to set position! Result: -3001
when I use rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 500}"
Probably because the Ax-18A use 1000000 bauds not 57600

When i try to use other baud-rates, the effects vary
Whit 1000000 i was able to move the second motor, (the one which is no attached to the U2D2) using id 1, but not the first one, and i keep getting errors [ERROR] [1653383772.792546507]: Failed to set position! Result: -3002(when launching rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 500}", but the motor moves this time)
and [ERROR] [1653383815.920792999]: Failed to set position! Result: -3001

Does the baud-rate affect the number of motor i can use? when i use 500000 or 400000 the motors dont move at all.

ROBOTIS-Will · 2022-05-25T01:03:46Z

@Alberto-D

Oh, you should modify the ID and baudrate for your AX settings.
By default the AX series use 1,000,000 bps as you mentioned.
The -3001 is the timeout error, so you can try setting your AX ID 1, and ID 2 to the same baudrate(such as 1000000) and modify the baudrate in the code accordingly.

Although AX series support various baudrates, I'd recommend to use baudrates that are widely used such as 57600, 115200, 1Mbps.

Thanks!

Alberto-D · 2022-05-27T08:59:10Z

This is my code rigth now:
// Copyright 2020 ROBOTIS CO., LTD.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*******************************************************************************

This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series with U2D2.
For other series, please refer to the product eManual and modify the Control Table addresses and other definitions.
To test this example, please follow the commands below.
Open terminal I got this problem in MatLab ...dxl/DXL_SDK_LINUX_v1_01/src/dynamixel.so: clase ELF errónea: ELFCLASS32 #1
$ roscore
Open terminal Different return delay time #2
$ rosrun dynamixel_sdk_examples read_write_node
Open terminal potential out-of-bounds write in Protocol1PacketHandler::WriteTxRx #3 (run one of below commands at a time)
$ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 0}"
$ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 1000}"
$ rosservice call /get_position "id: 1"
$ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 2, position: 0}"
$ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 2, position: 1000}"
$ rosservice call /get_position "id: 2"
Author: Zerom
*******************************************************************************/

#include <ros/ros.h>

#include "std_msgs/String.h"
#include "dynamixel_sdk_examples/GetPosition.h"
#include "dynamixel_sdk_examples/SetPosition.h"
#include "dynamixel_sdk/dynamixel_sdk.h"

using namespace dynamixel;

//For AX-18
// Control table address
#define ADDR_TORQUE_ENABLE 24
#define ADDR_GOAL_POSITION 30
#define ADDR_PRESENT_POSITION 36