11
11
12
12
class Person (Supervisor ):
13
13
def __init__ (self , person_number ):
14
+ """
15
+ initializes an instance of `Robot`, which is responsible for controlling
16
+ a humanoid robot's movement based on user input and BVH animation data.
17
+ It sets up various fields and methods for controlling the robot's position,
18
+ rotation, and velocity.
19
+
20
+ Args:
21
+ person_number (int): 3D human character that the function is supposed
22
+ to animate, and is used to load the appropriate motion data for
23
+ that character from the library `/usr/local/webots/projects/humans/skin_animated_humans/libraries/bvh_util/libbvh_util.so`.
24
+
25
+ """
14
26
parent = os .path .dirname (os .path .realpath (__file__ ))
15
27
main_directory = os .path .abspath (os .path .join (parent , os .pardir ))
16
28
print (main_directory )
@@ -43,6 +55,19 @@ def set_speed(self, data):
43
55
########## In case the receibed speed is in the person frame ##########
44
56
# if time.time() - self.last_time_sent_velocity > self.time_between_velocity_commands:
45
57
# self.last_time_sent_velocity = time.time()
58
+ """
59
+ takes in the velocity data of a person and calculates the orientation of
60
+ that person based on the axles of their body. Then it uses the calculated
61
+ orientation to convert the input velocity to world coordinates, and finally
62
+ sets the new velocity of the person's node in the scene using PySAM.
63
+
64
+ Args:
65
+ data (float): 6DOF (degree of freedom) velocity input for the person,
66
+ which is used to calculate the converted linear velocity in the
67
+ body-centered coordinate system and then passed to the `setVelocity()`
68
+ method of the `PersonNode`.
69
+
70
+ """
46
71
person_orientation = self .person_node .getOrientation ()
47
72
orientation = math .atan2 (person_orientation [0 ], person_orientation [1 ]) - math .pi / 2
48
73
rotation_matrix = np .array (
@@ -57,11 +82,30 @@ def set_speed(self, data):
57
82
[converted_speed [0 ] / 2.5 , converted_speed [1 ] / 2.5 , 0 , 0 , 0 , - data .axes [0 ].value * math .pi / 2 ])
58
83
59
84
def set_initial_pose (self , data ):
85
+ """
86
+ sets the initial position and orientation of an object based on button
87
+ input data.
88
+
89
+ Args:
90
+ data (`sf::Data` type in the given code snippet.): 3D transform of the
91
+ object at the start position, which is used to set the initial
92
+ pose of the robot when `if data.buttons[3]:` condition is met.
93
+
94
+ - `buttons`: A 32-bit integer vector representing button states
95
+ (0 for none, non-zero for pressed buttons).
96
+
97
+
98
+ """
60
99
if data .buttons [3 ]:
61
100
self .traslation_field .setSFVec3f (self .start_pose )
62
101
self .rotation_field .setSFRotation (self .start_rotation )
63
102
64
103
def get_camera_image (self ):
104
+ """
105
+ captures an image from a person's camera, converts it to RGB format, and
106
+ displays it using `cv2.imshow()`.
107
+
108
+ """
65
109
color = self .person_camera .getImage ()
66
110
color_image = cv2 .cvtColor (cv2 .cvtColor (
67
111
np .frombuffer (color , np .uint8 ).reshape (self .person_camera .getHeight (), self .person_camera .getWidth (), 4 ),
0 commit comments