-
Notifications
You must be signed in to change notification settings - Fork 2
/
f_formations.py
136 lines (126 loc) · 5.13 KB
/
f_formations.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
#!/usr/bin/python
"""
This file emulates some kinds of F Formations.
"""
import numpy as np
DEFAULT_CENTER = (0., 0., 0.)
DEFAULT_RADIUS = 0.6
DEFAULT_SIGMA = (0.1, 0.1, 0.1)
def Lshape(center=DEFAULT_CENTER, radius=DEFAULT_RADIUS, sigma=DEFAULT_SIGMA):
""" create F Formation of the type L
Arguments:
- `poses`: the pose where the new humans will be created
- `center`: list parameter (x,y,theta)
- `radius`: float parameter distance from the center of the
f_formation to the human
- `sigma`: for the error in positions diag(sigma_x,sigma_y,sigma_theta)
"""
x, y, theta = center
r = radius
poses = []
# person 1
sg_x, sg_y, sg_th = np.random.randn(3)*sigma
poses.append([x+r*np.cos(theta)+sg_x, y+r*np.sin(theta)+sg_y, theta+np.pi+sg_th])
# person 2
sg_x, sg_y, sg_th = np.random.randn(3)*sigma
poses.append([x+r*np.cos(theta+np.pi/2)+sg_x, y+r*np.sin(theta+np.pi/2)+sg_y,
theta-np.pi/2+sg_th])
return poses
def visAvis(center=DEFAULT_CENTER, radius=DEFAULT_RADIUS, sigma=DEFAULT_SIGMA):
""" create F Formation of the type vis-A-vis
Arguments:
- `poses`: the pose where the new humans will be created
- `center`: list parameter (x,y,theta)
- `radius`: float parameter distance from the center of the
f_formation to the human
- `sigma`: for the error in positions diag(sigma_x,sigma_y,sigma_theta)
"""
return circleFormation(2, center, radius, sigma)
def Vshape(center=DEFAULT_CENTER, radius=DEFAULT_RADIUS, sigma=DEFAULT_SIGMA):
""" create F Formation of the type V
Arguments:
- `poses`: the pose where the new humans will be created
- `center`: list parameter (x,y,theta)
- `radius`: float parameter distance from the center of the
f_formation to the human
- `sigma`: for the error in positions diag(sigma_x,sigma_y,sigma_theta)
"""
x, y, theta = center
r = radius
poses = []
# person 1
sg_x, sg_y, sg_th = np.random.randn(3)*sigma
poses.append([x+r*np.cos(theta)+sg_x, y+r*np.sin(theta)+sg_y, theta+np.pi+sg_th])
# person 2
sg_x, sg_y, sg_th = np.random.randn(3)*sigma
poses.append([x+r*np.cos(theta+3*np.pi/4)+sg_x, y+r*np.sin(theta+3*np.pi/4)+sg_y,
theta-np.pi/4+sg_th])
return poses
def Cshape(center=DEFAULT_CENTER, radius=DEFAULT_RADIUS, sigma=DEFAULT_SIGMA):
""" create F Formation of the type V
Arguments:
- `poses`: the pose where the new humans will be created
- `center`: list parameter (x,y,theta)
- `radius`: float parameter distance from the center of the
f_formation to the human
- `sigma`: for the error in positions diag(sigma_x,sigma_y,sigma_theta)
"""
x, y, theta = center
r = radius
poses = []
# person 1
sg_x, sg_y, sg_th = np.random.randn(3)*sigma
poses.append([x+r*np.cos(theta)+sg_x, y+r*np.sin(theta)+sg_y, theta+np.pi+sg_th])
# person 2
sg_x, sg_y, sg_th = np.random.randn(3)*sigma
poses.append([x+r*np.cos(theta+np.pi/4)+sg_x, y+r*np.sin(theta+np.pi/4)+sg_y,
theta-3*np.pi/4+sg_th])
return poses
def circleFormation(n=4, center=DEFAULT_CENTER, radius=DEFAULT_RADIUS, sigma=DEFAULT_SIGMA):
""" create F Formation of the type Circular with N people
Arguments:
- `n`: number of humans you want
- `poses`: the pose where the new humans will be created
- `center`: list parameter (x,y,theta)
- `radius`: float parameter distance from the center of the
f_formation to the human
- `sigma`: for the error in positions diag(sigma_x,sigma_y,sigma_theta)
"""
x, y, theta = center
r = radius
poses = []
for i in xrange(n):
# update random variables at each time
sg_x, sg_y, sg_th = np.random.randn(3)*sigma
# append pose
poses.append([x+r*np.cos(theta+i*2*np.pi/n+sg_x),
y+r*np.sin(theta+i*2*np.pi/n+sg_y),
theta+np.pi*(1+i*2./n)+sg_th])
return poses
def sideBySide(center=DEFAULT_CENTER, radius=DEFAULT_RADIUS, sigma=DEFAULT_SIGMA):
""" create F Formation of the type side-by-side
In this case, the people are not looking to the center of
the O-space (since they suppose to be walking)
Arguments:
- `poses`: the pose where the new humans will be created
- `center`: list parameter (x,y,theta)
- `radius`: float parameter distance from the center of the
f_formation to the human
- `sigma`: for the error in positions diag(sigma_x,sigma_y,sigma_theta)
"""
x, y, theta = center
r = radius
poses = []
# person 1
sg_x, sg_y, sg_th = np.random.randn(3)*sigma
poses.append([x+r*np.cos(theta)+r*.6*np.cos(np.pi/2+theta)+sg_x,
y+r*np.sin(theta)+r*.6*np.sin(np.pi/2+theta)+sg_y,
# theta+np.pi+np.pi/9+sg_th]) # with a litte twist
theta+np.pi+sg_th])
# person 2
sg_x, sg_y, sg_th = np.random.randn(3)*sigma
poses.append([x+r*np.cos(theta)-r*.6*np.cos(np.pi/2+theta)+sg_x,
y+r*np.sin(theta)-r*.6*np.sin(np.pi/2+theta)+sg_y,
# theta+np.pi-np.pi/9+sg_th]) # with a little twist
theta+np.pi+sg_th])
return poses