Skip to content

Commit

Permalink
Adds english comments to the remaining files
Browse files Browse the repository at this point in the history
  • Loading branch information
Shuhei Kozasa committed Jan 4, 2024
1 parent 3f8152c commit dbf3340
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 0 deletions.
11 changes: 11 additions & 0 deletions sciurus17_examples/scripts/preset_pid_gain_example.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,13 @@
def preset_pid_gain(pid_gain_no):
# サーボモータのPIDゲインをプリセットする
# プリセットの内容はsciurus7_control/scripts/preset_reconfigure.pyに書かれている
# Presets the PID gains of the servo motor
# The presets are written in sciurus7_control/scripts/preset_reconfigure.py
print("PID Gain Preset. No." + str(pid_gain_no))
preset_no = UInt8()
preset_no.data = pid_gain_no
pub_preset.publish(preset_no)
# Waits until the PID gain is set
rospy.sleep(1) # PIDゲインがセットされるまで待つ


Expand All @@ -36,27 +39,34 @@ def main():
arm.set_max_velocity_scaling_factor(0.1)

# サーボモータのPIDゲインをプリセット
# Presets the PID gain of the servo motor
preset_pid_gain(0)

# SRDFに定義されている"r_arm_init_pose"の姿勢にする
# Moves the robot the the "r_arm_init_pose" pose defined in the SRDF
print("set named target : r_arm_init_pose")
arm.set_named_target("r_arm_init_pose")
arm.go()

# サーボモータのPIDゲインをプリセット
# Pゲインが小さくなるので、Sciurus17の右手を人間の手で動かすことが可能
# Presets the PID gain of the servo motor
# You can move the Sciurus17's right arm because the P gain decreases
preset_pid_gain(4)

# 動作確認のため数秒間待つ
# Waits a couple seconds for operation confirmation
sleep_seconds = 10
for i in range(sleep_seconds):
print( str(sleep_seconds-i) + " counts left.")
rospy.sleep(1)
# 安全のため、現在の右手先姿勢を目標姿勢に変更する
# Updates the target pose to the current pose for safety reasons
arm.set_pose_target(arm.get_current_pose())
arm.go()

# サーボモータのPIDゲインをプリセット
# Presets the PID gain of the servo motor
preset_pid_gain(0)

print("set named target : r_arm_init_pose")
Expand All @@ -68,6 +78,7 @@ def main():

if __name__ == '__main__':
# PIDゲインプリセット用のPublisher
# The Publisher for the PID gain presets
pub_preset = rospy.Publisher("preset_gain_no", UInt8, queue_size=1)

try:
Expand Down
13 changes: 13 additions & 0 deletions sciurus17_examples/scripts/waist_joint_trajectory_example.py
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@

class WaistYaw(object):
# 初期化処理
# Initialize
def __init__(self):
self.__client = actionlib.SimpleActionClient("/sciurus17/controller3/waist_yaw_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
Expand All @@ -39,31 +40,38 @@ def __init__(self):
self.present_angle = 0.0

# 現在角度を設定
# Sets the current angle
def set_present_angle(self, angle):
self.present_angle = angle

# 目標角度の設定と実行
# Sets the target angle and executes
def set_angle(self, yaw_angle, goal_sec):
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ["waist_yaw_joint"]

# 現在の角度から開始(遷移時間は現在時刻)
# Starts from the current angle (The transition time is the current time)
yawpoint = JointTrajectoryPoint()
yawpoint.positions.append(self.present_angle)
yawpoint.time_from_start = rospy.Duration(nsecs=1)
goal.trajectory.points.append(yawpoint)

# 途中に角度と時刻をappendすると細かい速度制御が可能
# 参考=> http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement
# You can do a fine speed control if you append the angle and time in between
# Ref=> http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement

# ゴール角度を設定(指定されたゴール時間で到達)
# Sets the goal angle (Arrives at the specified goal time)
yawpoint = JointTrajectoryPoint()
yawpoint.positions.append(yaw_angle)
yawpoint.time_from_start = rospy.Duration(goal_sec)
goal.trajectory.points.append(yawpoint)
self.present_angle = yaw_angle

# 軌道計画を実行
# Executes the planned trajectory
self.__client.send_goal(goal)
self.__client.wait_for_result(rospy.Duration(goal_sec))
return self.__client.get_result()
Expand All @@ -75,16 +83,21 @@ def set_angle(self, yaw_angle, goal_sec):
wy.set_present_angle(math.radians(0.0))

# まず正面を向く
# Faces the front
wy.set_angle(math.radians(0.0), 0.1)
rospy.sleep(1.0)
# 左45度,1秒
# Left 45 degrees, 1 second
wy.set_angle(math.radians(45.0), 1.0)
rospy.sleep(1.0)
# 正面:1秒
# Fornt 1 second
wy.set_angle(math.radians(0.0), 1.0)
rospy.sleep(1.0)
# 右45度,1秒
# Right 45 degrees, 1 second
wy.set_angle(math.radians(-45.0), 1.0)
rospy.sleep(1.0)
# 正面,1秒
# Front 1 second
wy.set_angle(math.radians(0.0), 1.0)

0 comments on commit dbf3340

Please sign in to comment.