add resolution divider option to turn more #8
Conversation
| @@ -92,6 +91,7 @@ def __calib(self): | |||
| if self.torque_limit is not None: | |||
| self.set_torque_limit(self.torque_limit) | |||
| motor_states_sub_for_init.unregister() | |||
| init_pos = self.motor_states_for_init['position'] | |||
There was a problem hiding this comment.
This line should be before self.set_torque_enable(False) at least because self.set_torque_enable(False) makes motor torque off.
If init_pos is taken after that, init_pos may be different from limit position.
And self.set_torque_enable(False) cannot be deleted for my gripper because keeping torque on around limit makes the timing belt slipped.
I'm not sure whether this line should be after self.__set_speed_wheel(0.0).
If you can find another solution which does not change the original function order, I'm happy.
| @@ -56,6 +56,9 @@ def __init__(self, dxl_io, controller_namespace, port_namespace): | |||
| JointController.__init__(self, dxl_io, controller_namespace, port_namespace) | |||
|
|
|||
| self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id') | |||
| self.resolution_divider = rospy.get_param(self.controller_namespace + '/motor/resolution_divider', 1) | |||
| self.dxl_io.set_resolution_divider(self.motor_id, self.resolution_divider) | |||
There was a problem hiding this comment.
I think we should write this to all other controllers because default dynamixel_io.py considers resolution_divider as 1, which causes error when we use a motor having resolution_divider other than 1 with a controller other than joint_position_controller.
Perhaps dynamixel_io.py should read resolution_divider from motors at some timing.
Also, do you check your code with motors without multi-turn mode (e.g., AX-12)?
I assume this line raises an error.
| @@ -1018,6 +1020,7 @@ def get_feedback(self, servo_id): | |||
| position = response[11] + (response[12] << 8) | |||
| if position & 0x8000: | |||
| position += -0x10000 | |||
| position = position * self.resolution_divider | |||
There was a problem hiding this comment.
Sorry, I'm not confident about this change.
I assume this change makes commanded position and feedbacked position different.
Is this OK?
Or do I misunderstand something?
There was a problem hiding this comment.
yes this is ok.
this change is necessary.
without this change, feedback position and commanded position does not match.
There was a problem hiding this comment.
ごめんなさい、もっと詳しく教えてほしいんですが、なんで合わないんでしょうか。
feedback内のgoalは
https://emanual.robotis.com/docs/en/dxl/mx/mx-28/#goal-position
の値で、feedback内のpositionは
https://emanual.robotis.com/docs/en/dxl/mx/mx-28/#present-position
の値なんですが、どちらもresolution_dividerの値が同じように作用するように読めます。
- 修正なしだと起こる現象の詳細（rostopic echoの値とか？）
- その原因として考えられることは何か
を教えてください。
There was a problem hiding this comment.
resolution_dividerの値が同じように作用するように読めます。
https://www.besttechnology.co.jp/modules/knowledge/?MX%20Series%20Control%20table#hceb4e38
ここを読むと，positionはfeedback * resolution_dividerと書かれています．
ROBOTISのドキュメントは間違っている？（もしくは読み間違えなのか・・・）
この変更無しで実機で試すと全く合わないです．
0に移動させるよう指示しても0ではないどこかに移動してしまいます．
There was a problem hiding this comment.
https://www.besttechnology.co.jp/modules/knowledge/?MX%20Series%20Control%20table#hceb4e38
ここを読むと，positionはfeedback * resolution_dividerと書かれています．
ROBOTISのドキュメントは間違っている？
まず、besttechnologyのドキュメントにおける「真の位置フィードバック値」は「get_feedback関数の返り値」のことではないと思います。
besttechnologyのドキュメントにおける「フィードバック」というのは、モータ内部で回っている位置制御フィードバックのことを指していると思います。
一方、関数名の「feedback」というのは、モータと通信して得られる情報のことで、モータの仕様表にもFeedbackという名前で挙げられています。
https://emanual.robotis.com/docs/en/dxl/mx/mx-28/#specifications
いったん理屈上で考えてみます。
goal_positionやpresent_positionの範囲は-28672~28672（レジスタのビット数的にこれ以上にはならない）です。
Multi-turnモードではこれをフルで使って、正転方向と逆転方向にそれぞれ7回転を実現します。
https://emanual.robotis.com/docs/en/dxl/mx/mx-28/#goal-position
で、これを7回転以上させるのがresolution_dividerです。
例えばresolution_dividerが2の時、goal_positionに28672と入れると正転方向に14回転回るはずです。
2048と入れると正転方向に1回転します。
で、回り切った時、present_positionも2048のはずです。
これがもし4096（resolution_dividerが1の時と同じ値）だったら、14回転回そうとしたら途中でpresent_positionのレジスタは飽和してしまいます。
このことから、goal_positionとpresent_positionにはresolution_dividerが同じように作用するはずです。
もし作用しないのであれば、present_positionは飽和するか、桁あふれで値がおかしくなるはずです。
以上を踏まえた上で、besttechnologyのドキュメントですが、「真の位置フィードバック値を本値で乗じた値がPresent Positionに現れます」とありますが、「乗じた」ではなくて「除した」の誤りなのではないかと思います。
つまり、例えばresolution_dividerが2の時、1回転させたい時の位置制御の「真の」目標値は4096なんだけど、これをレジスタにそのまま入れるとレジスタが飽和する場合があるので、resolution_dividerで割った2048がpresent_positionに入る、ということだと思いました。
で、
この変更無しで実機で試すと全く合わないです． 0に移動させるよう指示しても0ではないどこかに移動してしまいます．
の現象についてですが、僕が怪しいと思っているのは、radian単位のrostopicで受け取った司令値をgoal_postionに変換したり、present_positionをradian単位のrostopicに変換する部分です。
ここで使ってる
RADIANS_PER_ENCODER_TICKやENCODER_TICKS_PER_RADIANはrosparamで取ってきていて、このrosparamは以下で設定されています。
この値をresolution_dividerに応じて変えるのが正しいのではないでしょうか。
なぜなら、例えばresolution_dividerが2になると、goal_positionが1変わった時の角度変化は2倍になるからです。
I add
resolution_divideroption to turn more