Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

add resolution divider option to turn more #8

Open
wants to merge 1 commit into
base: gripper-v6-devel
Choose a base branch
from
Open

add resolution divider option to turn more #8

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion dynamixel_controllers/src/dynamixel_controllers/calib_required_joint_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,6 @@ def __calib(self):
rate = rospy.Rate(50)
while not rospy.is_shutdown():
try:
init_pos = self.motor_states_for_init['position']
if abs(self.motor_states_for_init['load']) > self.detect_limit_load:
break
except KeyError:
Expand All @@ -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']
Copy link
Owner

@pazeshun pazeshun Oct 1, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


# Remember initial joint position
diff = init_pos - self.initial_position_raw
Expand Down
3 changes: 3 additions & 0 deletions dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init')
self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min')
self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max')
Expand Down
3 changes: 3 additions & 0 deletions dynamixel_driver/src/dynamixel_driver/dynamixel_io.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,7 @@ def __init__(self, port, baudrate, readback_echo=False):
self.port_name = port
self.readback_echo = readback_echo
self.ignored_errors = {}
self.resolution_divider = 1
except SerialOpenError:
raise SerialOpenError(port, baudrate)

Expand Down Expand Up @@ -416,6 +417,7 @@ def set_resolution_divider(self, servo_id, divider):
response = self.write(servo_id, DXL_RESOLUTION_DIVIDER, [divider])
if response:
self.exception_on_error(response[4], servo_id, 'setting resolution divider to %d' % divider)
self.resolution_divider = divider
return response
else:
raise UnsupportedFeatureError(model, DXL_RESOLUTION_DIVIDER)
Expand Down Expand Up @@ -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
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Author

@knorth55 knorth55 Oct 3, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes this is ok.
this change is necessary.
without this change, feedback position and commanded position does not match.

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ごめんなさい、もっと詳しく教えてほしいんですが、なんで合わないんでしょうか。
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の値が同じように作用するように読めます。

  1. 修正なしだと起こる現象の詳細（rostopic echoの値とか？）
  2. その原因として考えられることは何か

を教えてください。

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

resolution_dividerの値が同じように作用するように読めます。

https://www.besttechnology.co.jp/modules/knowledge/?MX%20Series%20Control%20table#hceb4e38
ここを読むと，positionはfeedback * resolution_dividerと書かれています．
ROBOTISのドキュメントは間違っている？（もしくは読み間違えなのか・・・）
この変更無しで実機で試すと全く合わないです．
0に移動させるよう指示しても0ではないどこかに移動してしまいます．

Copy link
Owner

@pazeshun pazeshun Oct 3, 2022 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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に変換する部分です。

dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py

Lines 194 to 197 in 7d10df4

def process_command(self, msg):
angle = msg.data
mcv = (self.motor_id, self.pos_rad_to_raw(angle))
self.dxl_io.set_multi_position([mcv])

dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py

Lines 119 to 122 in 7d10df4

def pos_rad_to_raw(self, pos_rad):
if pos_rad < self.min_angle: pos_rad = self.min_angle
elif pos_rad > self.max_angle: pos_rad = self.max_angle
return self.rad_to_raw(pos_rad, self.initial_position_raw, self.flipped, self.ENCODER_TICKS_PER_RADIAN)

dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py

Lines 173 to 178 in 7d10df4

def rad_to_raw(self, angle, initial_position_raw, flipped, encoder_ticks_per_radian):
""" angle is in radians """
#print 'flipped = %s, angle_in = %f, init_raw = %d' % (str(flipped), angle, initial_position_raw)
angle_raw = angle * encoder_ticks_per_radian
#print 'angle = %f, val = %d' % (math.degrees(angle), int(round(initial_position_raw - angle_raw if flipped else initial_position_raw + angle_raw)))
return int(round(initial_position_raw - angle_raw if flipped else initial_position_raw + angle_raw))

dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py

Line 180 in 7d10df4

self.joint_state.current_pos = self.raw_to_rad(state.position, self.initial_position_raw, self.flipped, self.RADIANS_PER_ENCODER_TICK)

dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_controller.py

Lines 180 to 181 in 7d10df4

def raw_to_rad(self, raw, initial_position_raw, flipped, radians_per_encoder_tick):
return (initial_position_raw - raw if flipped else raw - initial_position_raw) * radians_per_encoder_tick

ここで使ってるRADIANS_PER_ENCODER_TICKやENCODER_TICKS_PER_RADIANはrosparamで取ってきていて、

dynamixel_motor/dynamixel_controllers/src/dynamixel_controllers/joint_position_controller.py

Lines 80 to 81 in 7d10df4

self.RADIANS_PER_ENCODER_TICK = rospy.get_param('dynamixel/%s/%d/radians_per_encoder_tick' % (self.port_namespace, self.motor_id))
self.ENCODER_TICKS_PER_RADIAN = rospy.get_param('dynamixel/%s/%d/encoder_ticks_per_radian' % (self.port_namespace, self.motor_id))

このrosparamは以下で設定されています。

dynamixel_motor/dynamixel_driver/src/dynamixel_driver/dynamixel_serial_proxy.py

Lines 155 to 158 in 7d10df4

rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' %(self.port_namespace, motor_id), encoder_resolution / range_degrees)
rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' %(self.port_namespace, motor_id), encoder_resolution / range_radians)
rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' %(self.port_namespace, motor_id), range_degrees / encoder_resolution)
rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' %(self.port_namespace, motor_id), range_radians / encoder_resolution)

この値をresolution_dividerに応じて変えるのが正しいのではないでしょうか。
なぜなら、例えばresolution_dividerが2になると、goal_positionが1変わった時の角度変化は2倍になるからです。

error = position - goal
speed = response[13] + ( response[14] << 8)
if speed > 1023: speed = 1023 - speed
Expand Down