add resolution divider option to turn more

[knorth55]

I add resolution_divider option to turn more

[pazeshun]

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.

[pazeshun]

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.

[pazeshun]

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?

[knorth55]

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

[pazeshun]

ごめんなさい、もっと詳しく教えてほしいんですが、なんで合わないんでしょうか。
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. その原因として考えられることは何か

を教えてください。

[knorth55]

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

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

[pazeshun]

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倍になるからです。