reachy2021-unity-package-helloworld.py

#   Copyright POLLEN ROBOTICS
#
#   Licensed under the Apache License, Version 2.0 (the "License");
#   you may not use this file except in compliance with the License.
#   You may obtain a copy of the License at
#
#       http://www.apache.org/licenses/LICENSE-2.0
#
#   Unless required by applicable law or agreed to in writing, software
#   distributed under the License is distributed on an "AS IS" BASIS,
#   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
#   See the License for the specific language governing permissions and
#   limitations under the License.

from reachy_sdk import ReachySDK
from reachy_sdk.trajectory import goto
from reachy_sdk.trajectory import InterpolationMode
import time


def happy_antennas():
  for _ in range(7):
    reachy.head.l_antenna.goal_position = 30.0
    reachy.head.r_antenna.goal_position = -30.0
    
    time.sleep(0.2)
    
    reachy.head.l_antenna.goal_position = -30.0
    reachy.head.r_antenna.goal_position = 30.0
    
    time.sleep(0.2)
    
  reachy.head.l_antenna.goal_position = 0.0
  reachy.head.r_antenna.goal_position = 0.0

def hello_left_arm():
  left_arm_hello_position = {
    reachy.l_arm.l_shoulder_pitch: 0,
    reachy.l_arm.l_shoulder_roll: 50,
    reachy.l_arm.l_arm_yaw: 70,
    reachy.l_arm.l_elbow_pitch: -120,
    reachy.l_arm.l_forearm_yaw: 80,
    reachy.l_arm.l_wrist_pitch: 0,
    reachy.l_arm.l_wrist_roll: -40,
  }
  
  left_arm_base_position = {
    reachy.l_arm.l_shoulder_pitch: 0,
    reachy.l_arm.l_shoulder_roll: 0,
    reachy.l_arm.l_arm_yaw: 0,
    reachy.l_arm.l_elbow_pitch: 0,
    reachy.l_arm.l_forearm_yaw: 0,
    reachy.l_arm.l_wrist_pitch: 0,
    reachy.l_arm.l_wrist_roll: 0,
  }
  
  goto(
    goal_positions = left_arm_hello_position,
    duration = 1.0,
    interpolation_mode = InterpolationMode.MINIMUM_JERK
  )
  
  for _ in range(3):
    goto(
      goal_positions = {reachy.l_arm.l_wrist_roll: 20.0},
      duration = 0.5,
      interpolation_mode = InterpolationMode.LINEAR
    )
    goto(
      goal_positions = {reachy.l_arm.l_wrist_roll: -40.0},
      duration = 0.5,
      interpolation_mode = InterpolationMode.LINEAR
    )
    
  time.sleep(0.2)
  
  goto(
    goal_positions = left_arm_base_position,
    duration = 2.0,
    interpolation_mode = InterpolationMode.LINEAR
  )

def move_head():
  reachy.head.look_at(0.5, -0.3, -0.1, duration=1.0)
  time.sleep(0.3)
  reachy.head.look_at(0.5, 0.3, -0.1, duration=1.0)
  time.sleep(0.3)
  reachy.head.look_at(0.5, 0, -0.1, duration=1.0)
  time.sleep(0.3)

  head_tilted_position = {
    reachy.head.neck_roll: -20,
    reachy.head.neck_pitch: 0,
    reachy.head.neck_yaw:0,
  }

  goto(
    goal_positions = head_tilted_position,
    duration = 1.0,
    interpolation_mode = InterpolationMode.MINIMUM_JERK
  )

def say_hello():
  move_head()
  happy_antennas()
  hello_left_arm()
  reachy.head.look_at(0.5, 0, 0, duration=0.5)


if __name__ == "__main__":
  reachy = ReachySDK(host='localhost') # replace with correct IP if the simulation is not on your computer

  say_hello()