def remember(self, local_point):
"""
Set egocentric points to keep track of
"""
self.local_point = local_point
point = local_point.val(-1)
start_pos = self.robot_pose_node.val(-1)
a_T_o = n.transform2D(start_pos.pos, start_pos.angle)
self.global_point = np.linalg.inv(a_T_o) * ut.point_to_homo(point)
if (self.verbosity > 0):
print "V_KeepLocal_P2d_V: (", self.name, ") point ", point.T
print "V_KeepLocal_P2d_V: (", self.name, ") start_pos ", start_pos
print "V_KeepLocal_P2d_V: (", self.name, ") global_point ", self.global_point.T
def remember(self, local_point):
"""
Set egocentric points to keep track of
"""
self.local_point = local_point
point = local_point.val(-1)
start_pos = self.robot_pose_node.val(-1)
a_T_o = n.transform2D(start_pos.pos, start_pos.angle)
self.global_point = np.linalg.inv(a_T_o) * ut.point_to_homo(point)
if (self.verbosity > 0):
print "V_KeepLocal_P2d_V: (", self.name, ") point ", point.T
print "V_KeepLocal_P2d_V: (", self.name, ") start_pos ", start_pos
print "V_KeepLocal_P2d_V: (", self.name, ") global_point ", self.global_point.T
def calc(self, t): """ Return point(s) in the robot's current coordinate frame (frame b) i.e. bG Given: aG a_T_o b_T_o Calc: bG With: bG (b_T_o) (o_T_a) aG """ if (self.global_point == None): self.remember(self.local_point) current_pose = self.robot_pose_node.val(t) #print 'V_KeepLocal_P2d_V', current_pose b_T_o = n.transform2D(current_pose.pos, current_pose.angle) results = ut.homo_to_point(b_T_o * self.global_point) #print 'V_KeepLocal_P2d_V', results.T return results
def calc(self, t):
"""
Return point(s) in the robot's current coordinate frame (frame b)
i.e. bG
Given:
aG
a_T_o
b_T_o
Calc: bG
With: bG (b_T_o) (o_T_a) aG
"""
if (self.global_point == None):
self.remember(self.local_point)
current_pose = self.robot_pose_node.val(t)
#print 'V_KeepLocal_P2d_V', current_pose
b_T_o = n.transform2D(current_pose.pos, current_pose.angle)
results = ut.homo_to_point(b_T_o * self.global_point)
#print 'V_KeepLocal_P2d_V', results.T
return results
def calc(self, t): g = self.global_pos.val(t) l = self.local_pose.val(t) g_T_l = np.linalg.inv(n.transform2D(g.pos, g.angle)) return ut.homo_to_point(g_T_l * ut.point_to_homo(l))
def calc(self, t):
g = self.global_pos.val(t)
l = self.local_pose.val(t)
g_T_l = np.linalg.inv(n.transform2D(g.pos, g.angle))
return ut.homo_to_point(g_T_l * ut.point_to_homo(l))
