diff --git a/hackathon_demo/test_bathing.py b/hackathon_demo/test_bathing.py index 0c7c0bc8..fdcffa81 100644 --- a/hackathon_demo/test_bathing.py +++ b/hackathon_demo/test_bathing.py @@ -4,27 +4,45 @@ # Script to test bed bathing in hackathon. if __name__ == "__main__": + # connect with the Unity environment + # Change the path to the Unity executable file env = RCareWorld( - executable_file="/home/cathy/Workspace/RCareUnity/Build/Bathing/Ubuntu/bathing_ubuntu.x86_64" + executable_file="" ) # A collection of all force readings over all steps. all_nonzero_forces = [] - # robot = env.create_robot( - # id=12345, gripper_list=["123450"], robot_name="stretch3", base_pos=[0, 0, 0] - # ) + # Create a robot and a target object + # Control the robot arm + robot = env.create_robot( + id=123456, gripper_list=[123456], robot_name="stretch3", base_pos=[0, 0, 0] + ) + # Control the robot base + robot_base = env.create_robot(id = 12346, robot_name = 'mobile_base', base_pos = [0, 0, 1]) + # The red cuba in the environment target = env.create_object(id=2333, name="Cube", is_in_scene=True) + # Move the robot to the target object for i in range(10): + position = target.getPosition() + # print(position) + robot.BioIKMove(position) + env.step() + # Move the robot base, don't move faster than this + for i in range(100): + position = robot_base.getRobotState()["position"] + position[2]+=0.004 + print(position) + robot_base.setTransform(position) env.step() while True: position = target.getPosition() rotation = target.getRotation() - # robot.directlyMoveTo(position) + robot.directlyMoveTo(position) - # All new forces, assumed to be non-zero by Unity. + # force on the sponge forces = env.instance_channel.data[509]["forces"] - # New proportion, or None if not updated. + # painted area prop = env.instance_channel.data[509]["proportion"] msg = "" if len(forces) > 0: @@ -43,4 +61,4 @@ msg += "*************" print(msg) - env.step() + env.step() \ No newline at end of file