[4-wheel Drive with Workbench_Controller/Wheel_Operator (Linear/Angular)]

[Ashok12698]

ISSUE TEMPLATE ver. 1.0.0

Before you open issue, please refer to ROBOTIS e-Manual

1. How to setup? (ex, U2D2, OpenCR,...)
   4 dynamixel servos connected in daisychain manner to RaspberryPi via U2D2+powersupply

2. Which Dynamixel have you used? and how many? (Please describe below format to all connected Dynamixels)

   • Model Name 1060

   • ID 1, 2, 3, 4

   • Baud Rate of Dynamixels 57600

   • Protocol Version 2.0

3. Write down the commands you used in order
   $roslaunch dynamixel_workbench_controllers dynamixel_controllers.launch

4. Copy and Paste your error message on terminal
... logging to /home/pi/.ros/log/a35592c8-4246-11ea-a673-b827eb16b4c4/roslaunch-raspberrypi-10432.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://raspberrypi:34013/

SUMMARY
========

PARAMETERS
* /dynamixel_info: /opt/ros/kinetic/...
* /dynamixel_workbench/dxl_read_period: 0.01
* /dynamixel_workbench/dxl_write_period: 0.01
* /dynamixel_workbench/mobile_robot_config/radius_of_wheel: 0.033
* /dynamixel_workbench/mobile_robot_config/seperation_between_wheels: 0.16
* /dynamixel_workbench/publish_period: 0.01
* /dynamixel_workbench/use_cmd_vel_topic: False
* /dynamixel_workbench/use_joint_states_topic: True
* /dynamixel_workbench/use_moveit: False
* /rosdistro: kinetic
* /rosversion: 1.12.14

NODES
/
  dynamixel_workbench (dynamixel_workbench_controllers/dynamixel_workbench_controllers)

auto-starting new master
process[master]: started with pid [10444]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to a35592c8-4246-11ea-a673-b827eb16b4c4
process[rosout-1]: started with pid [10457]
started core service [/rosout]
process[dynamixel_workbench-2]: started with pid [10464]
[ INFO] [1580268182.663404376]: Name : pan, ID : 1, Model Number : 1060
[ INFO] [1580268182.704071401]: Name : tilt, ID : 2, Model Number : 1060
[ INFO] [1580268182.795193799]: [DynamixelDriver] Succeeded to add sync write handler
[ INFO] [1580268182.795894469]: [DynamixelDriver] Succeeded to add sync write handler

^C[dynamixel_workbench-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

5. Please, describe detailedly what difficulty you are in 

  - It only detect 2 xl430w250 at a time. since i connected 4 xl430w250 servos, its not showing all if them...why so? what are the changes i need to do to configure this.
I checked with _"rosrun dynamixel_workbench_controllers find_dynamixel /dev/ttyUSB0"_ which shows all the 4 servos connected in daisychain manner correctly.
Please suggest!

[ROBOTIS-Will]

@Ash-S
The default launch file will load basic.yaml file to setup the DYNAMIXEL.
Please update the yaml file so the controller can identify all 4 DYNAMIXELs.
Thank you.

[Ashok12698]

@ROBOTIS-Will Yes, I updated and ran it accordingly but failed to detect all 4 dynamixels. Any other way to check that? Thanks!

[ROBOTIS-Will]

Not sure how you did it, but please see below result with my basic.yaml setup.

pan:
  ID: 1
  Return_Delay_Time: 0
tilt:
  ID: 2
  Return_Delay_Time: 0
roll:
  ID: 3
  Return_Delay_Time: 0
pitch:
  ID: 4
  Return_Delay_Time: 0

[Ashok12698]

Hi @ROBOTIS-Will , I tested it multiple times but result is same. is there anything i am missing to configure. Even though i have connected 4 dynamixels xl430w250 in daisy-chain manner (IDs: 1,2,3,4) with set baudrate 57600, but 'roslaunch' only detects 2 dynamixels with IDs 1 and 2.

process**[dynamixel_workbench-1]:** started with pid [13442]
[ INFO] [1590998233.045844833]: Name : pan, ID : 1, Model Number : 1060
[ INFO] [1590998233.087517328]: Name : tilt, ID : 2, Model Number : 1060
[ INFO] [1590998233.179630158]: [DynamixelDriver] Succeeded to add sync write handler
[ INFO] [1590998233.181138582]: [DynamixelDriver] Succeeded to add sync write handler

Is that the issue, as i can see "dynamixel_workbench-2" in your test result while its showing "dynamixel_workbench-1" in my case. Please check and suggest. Thanks.

[Ashok12698]

Hi @ROBOTIS-Will ; Fortunately, I found my mistake and resolved it to detect all 4 dynamixels.
since i configured all the 4 dynamixels same using dynamixel wizard, but still all the 4 dynamixels are running in a nonuniform manner. This happens when Irun this:
"roslaunch dynamixel_workbench_operators wheel_operator.launch"
Could you please tell me where all the dynamixels configuration is defined in source file or so, so that I would check more details about the dynamixels read/write configuration?
Thanks!

[ROBOTIS-Will]

Hi,
The wheel operator does not set any DYNAMIXEL, but only publishes cmd_vel. You must configure each DYNAMIXEL before running this.
You can refer to k_Read_Write.cpp example in the dynamixel_workbench_toolbox to see how to read to and write from a certain address.
Thank you.

[Ashok12698]

Hi @ROBOTIS-Will , Thanks for your kind reply. I do understand.
well, what i mean is that when i apply the command: "roslaunch dynamixel_workbench_operators wheel_operator.launch", i can see the dynamixel running until i start controlling it from keys as defined in code at "wheel_operator.cpp" but still only 2 (out of 4) of dynamixels can be controlled according to the 'wheel_operator' launch. The status is like:
ID 1: runs according to 'wheel operator' launch.
ID 2: Doesn't run at all
ID 3: Auto run once 'wheel operator.launch' launches. (no control)
ID 4: runs according to 'wheel operator' launch.

So i wish to know where the dynamixel configuration is defined for the connected dynamixels when I launch the 'wheel operator'. Is it defined in 'dynamixel_workbench_toolbox' ?. please suggest. Thanks!

[ROBOTIS-Will]

@Ash-S
Hi,
DYNAMIXELs are configured in the initControlItems() in dynamixel_workbench_controllers.cpp based on the content in yaml file.
Please note that the wheel operator is written for 2 wheel configuration, so in order to use it for 4 wheels, you should modify the code accordingly.
Thank you.

[Ashok12698]

Hi @ROBOTIS-Will , Thanks. 👍
well as i can see, the dynamixel_workbench_controllers.cpp works fine as it can detect all the 4 dynamixels connected in daisy-chain as shown below.

since i need to verify 'wheel_operator' for 4 dynamixels, Is that you mean to modify' wheel_operator.launch' OR 'wheel_operator.cpp' . Thank you!

[Ashok12698]

Hi @ROBOTIS-Will , I am sorry but I rechecked it. I didn't find where else to define the all the connected dynamixels except in wheel_2_0.yaml, which i already defined. also i can see all connected dynamixels while running dynamixel_workbench_controllers launch. Please suggest!

[ROBOTIS-Will]

@Ash-S
Hi,
Changing the source from 2 wheel to 4 wheel will take quite amount of effort and customization of the code that I cannot help you with. The source code we provide is calculating each wheel's rotation based on given rotational and transitional values, and you'll need to do the same for the 4 wheel movement. Maybe you can start with using identical calculation for wheels on each side.

[Ashok12698]

@ROBOTIS-Will Hi, Thanks! I do understand this well. Since i am new to ROS handling, could you please help me to tell where is this 2 wheels information defined and this identical calculation. is it 'dynamixel_workbench_controller.cpp' or its somewhere 'wheel_operator.cpp". Just need your guidelines to proceed on this. Thanks!

[ajsampathk]

Since this issue is still open I'll try and answer your question.
I had to modify the code to run 4 dynamixels as well and what @ROBOTIS-Will said is correct, youll need to modify the source code a bit.

dynamixel_workbench_controllers.cpp is the file you will need to modify.

If you go to Line 506 as of this version, you will find the
DynamixelController::commandVelocityCallback(const geometry_msgs::Twist::ConstPtr &msg) function which is basically where all the magic happens.
You will see LEFT and RIGHT variables which refer to the 2 motors in the config.

Now, you will need to add two more variables say, LEFT_REAR=2 and RIGHT_REAR=3. Note, the order of the dynamixels might change so you'll have to shuffle these values a bit to find out if you have the right motor associated with the right variable.

Here is the tricky bit:

double velocity_constant_value = 1 / (wheel_radius_ * rpm * 0.10472);

wheel_velocity[LEFT] = robot_lin_vel - (robot_ang_vel * wheel_separation_ / 2);
wheel_velocity[RIGHT] = robot_lin_vel + (robot_ang_vel * wheel_separation_ / 2);

This piece is the calculation is what you might want to change, since my 4 wheel config can be controlled by 2 velocities on each side I just copied the value from wheel_velocity[LEFT] to wheel_velocity[LEFT_REAR] and same for the right motors. something like this added to the above snippet:

wheel_velocity[LEFT_REAR]=wheel_velocity[LEFT];
wheel_velocity[RIGHT_REAR]=wheel_velocity[RIGHT];

Next, the tedious bit, is you'll have to replicate every line of code within the function that have the LEFT and RIGHT variables,
and modify the new copy of the line to have LEFT_REAR and RIGHT_REAR instead of LEFT and RIGHT. Make sure you did it for all lines (Or at least the ones that applies to your case ie, protocol 1,2 etc)

If you have done that right, compile with catkin_make and launch, you should have all 4 motors running properly.

One more thing:
If you've set the Baud to 57600 or less and are using the wheel_operator node, you will have a status packet error when reading the dynamixel states of 2 of the motors. This is because the wheel_operator has a publish rate of 100Hz and the controller node cannot handle 4 motors with low bauds at that publish rate, so lower the publish rate (30Hz seems to work for me). you wil have to do that in the wheel_operator.cpp file here and modify this line:

ros::Rate loop_rate(100);

[ROBOTIS-Will]

@ajsampathk Thank you for providing details! Your code is what I intended to explain.
@Ash-S I'm sorry about the late reply. Using identical value for the front and rear wheels will work with some hiccups for example, rotating in place and running a curve. Each wheel won't precisely track the route and could slip or jerky a bit. This could be worsen if the platform is running on a surface with a large friction.

[Ashok12698]

Hi Abhijith @ajsampathk & @ROBOTIS-Will
Thank you so much for your well detailed explanation and valuable suggestions. I look forward it and I can get all dynamixels moving but I stuck to understand that this shows me error in series (data read failed) like...

Please suggest me. Thanks.

[ajsampathk]

@Ash-S Hi, have you lowered the rate in wheel_operator.cpp ? Change the
ros::Rate loop_rate(100)
line to a lower rate, 30 should have no problems:
ros::Rate loop_rate(30)

OR, this is untested but you can try and increase the baud rate of your Dynamixels, to 1M maybe?

[Ashok12698]

Hi @ajsampathk & @ROBOTIS-Will , thanks for your suggestions. I tried this and it works well some times. May I please know, Is it the 'rate' at which the 'loop' gets called or any other? please brief.

Also, may i please understand, where are the msgs 'linear velocity' and 'angular velocity' defined. since i went through geometry_msgs..../Twist.h, but didn't understand it well. I mean, where I can define to run the particular servo on 'linear vel' and on 'angular vel'? please suggest. Thanks.

[ajsampathk]

The rate referes to the publish rate of the /cmd_vel topic from the wheel operator node.

And I'm not sure what you're asking with the velocities?

[Ashok12698]

Hi @ajsampathk , thanks for make me understand about it.. I checked it using 'rostopic echo /dynamixel_workbench_controller/cmd_vel and it shows the x,y,z values while controlling the wheels in linear and angular direction correspondingly.
About the /Twist.h', I mean to understand that where the motor rotation direction is defined in case of linear and angular. I can see it like this...

wheel_velocity[LEFT_F] = robot_lin_vel - (robot_ang_vel * wheel_separation_/2);
wheel_velocity[RIGHT_F] = robot_lin_vel + (robot_ang_vel * wheel_separation_/2);
wheel_velocity[LEFT_R] = robot_lin_vel - (robot_ang_vel * wheel_separation_/2);
wheel_velocity[RIGHT_R] = robot_lin_vel + (robot_ang_vel * wheel_separation_/2);

when I tested it, based on wheel operator roslaunch, in case of linear motion as in 'rostopic /cmd_vel', it moves angularly while in case of angular motion, it moves linearly. Is it the '/config/wheel_2_0.yaml', where the rotation direction should be defined (like Drive_Mode : 0 or 1 or any other, please suggest. Thanks.

[ajsampathk]

Yes, the angular and linear velocities seem to have been swapped, I had just swapped it back in the source myself so I didn't remember, the wheel configs have nothing to do with it as far as I know (unless the calculation makes it so that it gets swapped at a certain wheel separation value).

You can just swap the values for wheel_velocity[RIGHT_*] with wheel_velocity[LEFT_*] and it should be fine.

PS: Please change the name of the issue from [dynamixel_workbench_controller ROSlaunch with xl430w250]
to something to the effect of "4 wheel drive with workbench controller and wheel operator" so people with the same issue can find this thread. Thanks.

[Ashok12698]

@ajsampathk Thanks you for your valuable suggestions. well, since i tried to manage the twist msg, but it shows me the radial rotation, but i wish to manage it axial rotation. I mean, i wish to twist it on x axis, so i tried it like....

double robot_lin_vel = msg->linear.x
double robot_ang_vel = msg->angular.x

In the wheel operator, I tried it with like twist_msg.angular.x += ang_vel_step and twist_msg.angular.x -= ang_vel_step

After this when i tested, i found that it rotate circularly not axially on x-axis (like a screw rotation) while keeping y and z coordinates zero. Please suggest. Thanks. :

PS: Sure, I do the suggested changes in the post title. 👍

[ajsampathk]

Hi @Ash-S , Thanks for changing the title.

Do you mean to say the bot does'nt pivot in x-axis?

To rotate or pivot in place, you have to only supply the angular values while the linear values are zero. if there are constant linear and angular values the bot will move in a circular trajectory.

[Ashok12698]

Hi @ajsampathk, Yes it doesn't pivot on X-axis. since i changed the msg value in wheel operator like....
In case of LEFT, twist_msg.angular.x += ang_vel_step; and in case of RIGHT, twist_msg.angular.x -= ang_vel_step; because the linear vel defines linear (x,y,z) and angular (x,y,z) as well. so if i set linear.x and angular.x, it should show linear motion in x-axis and angular motion in x-axis (twist/pivot on x-axis it self), Isn't it? but the result is different (similar to a circular move).
What could be the scenario in case of 3-leg (120 aprt) with 3 wheel design? I am a bit confused to define that in case of 3 wheels, how it can be related to maintain the angular vel so that it can twist on its axis only. please suggest. Thanks.

[Ashok12698]

Hi @ajsampathk, any suggestions on this, please?

[ajsampathk]

Hi, as far as I know you can only move linearly in linear.x axis and angularly on angular.z axis.

I'm not sure if it is even possible to change that with normal 4-wheeled drives because it's only physically possible to move in those axes.

[Ashok12698]

Hi @ajsampathk , Thanks for your comment and suggestions so far. I'd dig more to this search. Still looking for some help to move on~