joint_state topic value not accurate

inhok996 commented 8 years ago

Hello~! I've been making my ev3 autonomous using ROS. I used motors.launch so that I could use the topics which I can make my ev3 move(outPortA(B)/command) and construct odometry information(joint_states). motors.launch worked successfully but I have an issue. When I publish minus value to /ev3dev/outPortA(B)/command, joint_state position and velocity value increase. This makes me hard to make node for odometry.

cyborg-x1 commented 8 years ago

I will see if I have time on the weekend to check this issue ...

cyborg-x1 commented 8 years ago

Hi Inho

Sorry, currently I am a bit busy ... I uploaded my Yocto build setup as I read your mail the first time on the other repo in my H4R Organisation...

https://github.com/Hacks4ROS/h4r_ev3_build

I will check for the sign issue, as soon as I can.

Regards,

Christian

On 02.08.2016 01:59, inhok996 wrote:

Hello~! I've been making my ev3 autonomous using ROS. I used motors.launch so that I could use the topics which I can make my ev3 move(outPortA(B)/command) and construct odometry information(joint_states). motors.launch worked successfully but I have an issue. When I publish minus value to /ev3dev/outPortA(B)/command, joint_state position and velocity value increase. This makes me hard to make node for odometry.

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/Hacks4ROS/h4r_ev3_ctrl/issues/3, or mute the thread https://github.com/notifications/unsubscribe-auth/AAp2WFDVdyKA5FPTY-qEnhlffW4CrSJLks5qbohqgaJpZM4JaHTx.

croesmann commented 8 years ago

Hi,

I can confirm this issue. I am using the motors.launch file along with motors.yaml. The issue can be reproduced by setting the mode of e.g. _JointA to position:

Ev3devJoints:
 Joint_A:
  speed_pid: [1001, 61 , 1] 
  mode: position

and by selecting the position controller (ros_control):

OutPortA:
  type: position_controllers/JointPositionController
  joint: Joint_A

Obviously, we must start our controllers, e.g. by loading the _controllerspawner with args="OutPortState OutPortA" in motors.launch.

If I now send positions to the controller, e.g. alternating +1,-1,.., the received joint angle via the joint_states topic is still positively increasing.

By the way, thank you for this awesome project!

Regards,

Christoph

cyborg-x1 commented 8 years ago

Currently I do not have a EV3 at hand.

Could one of you check if the value in /sys/class/tacho-motor/motorX/position is actually negative? Like cat /sys/class/tacho-motor/motorX/position - if it is not ... there is a driver problem - if it is, well then it is in the code and I probably used unsigned instead of signed somewhere or something like that...

croesmann commented 8 years ago

thanks, I tried that and /sys/class/tacho-motor/motorX/position is strictly increasing as well even for negative values. So it should be a driver issue. It is interesting that the controller actually drives the motor to the correct position (either left or right), just the returned position is positive only.

Edit: With ros_control / ev3_manager turned off, the position /sys/class/tacho-motor/motorX/position can be negative/decreasing in case I manually rotate the motor by hand.

inhok996 commented 8 years ago

Even if ros_control/ev3_manager turned on, /sys/class/tacho-motor/motorX/position must be negatively dcreased by force and jointstate position and velocity value negatively increase as well. I checked it out.

croesmann commented 8 years ago

I think I have found the reason for the issue in joint_settings.h. The problem is that for negative commands, the controller's write command changes the polarity of the ev3 motor and commands the absolute value to the motor. This causes the motor to change its positive turning direction (which indeed forces the motor to rotate the other way round) but the encoder values are still positive (which is reasonable since the positive direction has been redefined).

In order to fix this, the negative speed or position (depending on the control mode) can be commanded directly to the tacho-motor driver without changing polarity.

The issue can be reproduced by trying the following commands directly with the ev3 tacho-motor driver (connect to the ev3 via ssh, but do not start the ev3_manager):

export MC=/sys/class/tacho-motor/motor0
echo 100 > $MC/duty_cycle_sp # similar to joint_settings.h
echo 200 > $MC/speed_sp
echo on > $MC/speed_regulation
echo run-forever > $MC/command
# now $MC/position  increases and $MC/speed is positive
echo -200 > $MC/speed_sp
echo run-forever > $MC/command
# now $MC/position decreases and $MC/speed is negative
echo 200 > $MC/speed_sp
echo run-forever > $MC/command
# let's change the polarity like in joint_settings.h
echo inversed > $MC/polarity
echo run-forever > $MC/command
# the motor turns its direction but $MC/position is still increasing

I would try to recompile the yocto image on my own in order to try out changes in the controller, but I haven't looked into compiling https://github.com/Hacks4ROS/h4r_ev3_build from scratch yet. Is it straight forward and where can I find any compile commands?

Some additional findings that I have noticed during code checking:

For velocity control mode, a new speed value is commanded by updating speed_sp, duty_cycle_sp and by reinvocation of run-forever. However, http://www.ev3dev.org/docs/tutorials/tacho-motors/ suggests to use run-direct in combination with a positive or negative duty-cycle in order to apply quick updates. But the current solution should work fine as long as the controller rate is sufficient.
Not related to your project but to ev3dev: using the position control mode by commanding run-to-abs-pos does not force the motor to stop at position_sp. In case of high velocities the motor cannot break in time and thus the resulting position is incorrect. I am thinking of using the velocity interface in combination with a position controller implemented in ros_control rather using the ev3dev position mode. Or have I missed any configuration parameter?

Thank you for your support!

Best regards,

Christoph

cyborg-x1 commented 8 years ago

Hi again, I just removed the inversion "feature", you can try it now by installing the package on your brick. Maybe you need to force dpkg but I don't think so. See the releases section.

Btw. As mentioned before I could not test it ;-)

Regards,

Christian

croesmann commented 8 years ago

Thank you for the fix. Unfortunately it does not work. I think it's because you have compiled and packaged _h4r_ev3control instead of _h4r_ev3manager. But the hotfix actually affects the manager node code.

See contents of the deb file:

root@Brick1:~/h4r_deb# dpkg-deb -c h4r-ev3-control_0.1-r0_armel.deb 
drwxrwxrwx 0/0         0 2016-02-13 18:11:22 ./
drwxr-xr-x 0/0         0 2016-02-13 18:11:18 ./opt/
drwxr-xr-x 0/0         0 2016-02-13 18:11:18 ./opt/ros/
drwxr-xr-x 0/0         0 2016-02-13 18:11:18 ./opt/ros/indigo/
drwxr-xr-x 0/0         0 2016-02-13 18:11:18 ./opt/ros/indigo/share/
drwxr-xr-x 0/0         0 2016-02-13 18:11:18 ./opt/ros/indigo/share/h4r_ev3_control/
-rw-r--r-- 0/0      1343 2016-02-13 18:10:58 ./opt/ros/indigo/share/h4r_ev3_control/ev3_controller_plugins.xml
-rw-r--r-- 0/0      1294 2016-02-13 18:10:58 ./opt/ros/indigo/share/h4r_ev3_control/package.xml
drwxr-xr-x 0/0         0 2016-02-13 18:11:18 ./opt/ros/indigo/lib/
-rwxr-xr-x 0/0     94852 2016-02-13 18:11:18 ./opt/ros/indigo/lib/libev3_control.so
-rwxr-xr-x 0/0    261324 2016-02-13 18:11:18 ./opt/ros/indigo/lib/libev3_controllers.so

cyborg-x1 commented 8 years ago

Oh yes ... right sorry ... fixed that now

croesmann commented 8 years ago

Do you have the debian package containing the binary in non-debug mode as well? Then I would be able to test the hotfix.

I have only found the dev and dbg package here https://github.com/Hacks4ROS/h4r_ev3_ctrl/releases/tag/hf-i3-0.1

Thanks again for the hot-fix and your quick support!

EDIT: By trying to run the ev3_manager compiled in debug mode (_h4r-ev3-manager-dbg_0.1-r0armel.deb), I get the following error: cannot execute binary file: Exec format error. Could it be related to the ARM cross-compiler or just the debug mode? The binary architecture seems to be adequate: armv5te.

cyborg-x1 commented 8 years ago

What the ... did I do ... well probably I selected the wrong file to upload ... upload file selector is somehow a bit broken in Kubuntu ... now it is there, sorry for that.

croesmann commented 8 years ago

no problem! ;-) I was able to install the deb successfully.

Unfortunately, the issue hasn't been resolved. I wanted to check the packaged source code, if the fix is actually in the hot-fix deb. The dev-package does not include the header-files. Hence I looked into the dbg-package. The related file /usr/src/debug/h4r-ev3-manager/0.1-r0/git/h4r_ev3_manager/include/h4r_ev3_joint_setup/ev3_joint_settings.h does not seem to have the changes from commit 7d84947d3e68bb7357a37 (branch hotfix_negative_position) applied.

Could the same apply to the non-debug deb-package?

cyborg-x1 commented 8 years ago

Ok now I got it to compile ... I checked that now it should have the change inside. Saw it in the debug package. Previously it compiled the old version.

croesmann commented 8 years ago

Hi Christian, thank you, I have tried the new package and it definitely has the new code. But unfortunately, there is still a bug that must be resolved:

Now it is not possible to drive backwards anymore. Whenever I command a negative velocity, the command is not successfully written to the sys-file. I checked that in the following manner:

Publishing a negative velocity via ros_control velocity forwarding
Reading the commanded velocity via cat /sys/class/tacho-motor/motorX/speed_sp
For negative velocities, the commanded value is not written actually.

I found the reason for that in your code, it's just a minor mistake which can be fixed easily I guess: The issue occures in function writeIntToSysFile(...) in file h4r_ev3_ctrl/h4r_ev3_control/src/h4r_ev3_control/syshelpers.cpp. Here the signed integer is converted to a string in the following way:

std::string out;
bool negative= (value<0);
for (int i = 0; value >0 ; ++i) # the issue is here: for value<0, the loop is not executed
{
   out+=(value%10)+'0';
   value/=10;
}
if(negative)
   out+='-';

out=std::string ( out.rbegin(), out.rend() );
out+='\n';

Hence, for negative values, only the "-" sign is written to the sys-driver. A fix would be to perform the loop with the absolute value after determining if value is negative.

I can also make a pull-request into the hotifx branch if you like. But it's just a minor change.

Big thanks again!

PS: are you compiling the package with a simple cross-compiler resp. a docker image? Or do I need to get the full yocto layer compiled in order to add further non-included ros_controllers?

cyborg-x1 commented 8 years ago

Check now, I changed the function now this way:

       bool negative=false;
       if(value<0)
       {
           negative = true;
           value=-value;
       }

cyborg-x1 commented 8 years ago

@croesmann Does it work now?

croesmann commented 8 years ago

Hey Christian,

yes, the velocity mode works like a charm now (forward and backwards). The received positions are correct! Thank you!

There is still an issue with the position mode. But I think it is not related to this topic here, so I open a new issue report. This one might be closed.

Hacks4ROS / h4r_ev3_ctrl

joint_state topic value not accurate #3