preset_pid_gain_example サンプルの挙動を安定させたい

現状の問題点

preset_pid_gain_exampleでは、各ジョイントのPIDゲインを下げ、10秒経過後にPIDゲインを戻すという動作をしている。
PIDゲインを戻す直前に、set_pose_target() & go()で現在の姿勢を目標姿勢に変更し、
激しい挙動にならないようにしている。

しかし、set_pose_target() & go()に失敗する可能性もあるので、修正したい。

sciurus17_ros/sciurus17_examples/scripts/preset_pid_gain_example.py

Lines 38 to 45 in cf714f7

    
           for i in range(sleep_seconds): 
        
               print("sleep " + str(sleep_seconds-i) + " seconds.") 
        
               rospy.sleep(1) 
        
           # 安全のため、現在の右手先姿勢を目標姿勢に変更する 
        
           print("set target : current arm pose") 
        
           arm.set_pose_target(arm.get_current_pose()) 
        
           arm.go()

改善案

PIDゲインを落とした後も、for文の中で毎回目標姿勢を更新する。

# X7を手で動かせることを確認するため、数秒間待つ
    sleep_seconds = 10
    for i in range(sleep_seconds):
        rospy.loginfo("sleep " + str(sleep_seconds-i) + " seconds.")
        arm.set_pose_target(arm.get_current_pose())
        arm.go()
        rospy.sleep(1)

代替案

set_pose_target() & go()に失敗する原因を見つけられたら、そちらを対策する。

	for i in range(sleep_seconds):
	print("sleep " + str(sleep_seconds-i) + " seconds.")
	rospy.sleep(1)

	# 安全のため、現在の右手先姿勢を目標姿勢に変更する
	print("set target : current arm pose")
	arm.set_pose_target(arm.get_current_pose())
	arm.go()