def to_polydata(self, bottle_detected=False):
"""Converts the sensor to polydata."""
d = DebugData()
origin = np.array([self._state[0], self._state[1], self.z_distance])
for hit, intersection, ray in zip(self._hit,
self._intersections,
self._rays):
if hit:
color = [1., 0.45882353, 0.51372549]
endpoint = intersection
else:
color = [0., 0.6, 0.58823529]
endpoint = origin + ray
d.addLine(origin, endpoint, color=color, radius=0.05)
# add vision sensor
center = [self._state[0], self._state[1], 0]
axis = [0, 0, 1]
if bottle_detected:
color = [0., 0.8, 0.]
else:
color = [0., 0.6, 0.58823529]
d.addCircle(center, axis, 5, color=color)
return d.getPolyData()
def __init__(self, points, polyline=True, circle=None):
"""Constructs an Obstacle.
Args:
:type points: point array for polyline obstacle
:type polyline: if obstacle a polyline or not
:type circle: if obstacle in circular shape or not
"""
data = DebugData()
if polyline:
polydata = self.add_vtk_polygon(points)
polydata = filterUtils.appendPolyData([polydata])
elif circle is not None:
center = [0, 0, 0]
axis = [0, 0, 1] # Upright cylinder.
data.addCylinder(center, axis, 0.3, 0.1)
polydata = data.getPolyData()
else:
center = [points[0], points[1], -1]
axis = [0, 0, 1] # Upright cylinder.
data.addCircle(center, axis, 5)
polydata = data.getPolyData()
self._state = np.array([0., 0., 0.])
self.altitude = 0.
self._velocity = 0.
self._raw_polydata = polydata
self._polydata = polydata
def doFTDraw(self, unusedrobotstate):
frames = []
fts = []
vis_names = []
if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and
self.robotStateJointController.lastRobotStateMessage):
if self.draw_right:
rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque)
rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis
rft[1] = -1.*rft[1]
rft[3] = -1.*rft[3]
rft[4] = -1.*rft[4]
rft -= self.ft_right_bias
fts.append(rft)
frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque'))
vis_names.append('ft_right')
if self.draw_left:
lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force +
self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque)
lft -= self.ft_left_bias
fts.append(lft)
frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque'))
vis_names.append('ft_left')
for i in range(0, len(frames)):
frame = frames[i]
ft = fts[i]
offset = [0., 0., 0.]
p1 = frame.TransformPoint(offset)
scale = 1.0/25.0 # todo add slider for this?
scalet = 1.0 / 2.5
p2f = frame.TransformPoint(offset + ft[0:3]*scale)
p2t = frame.TransformPoint(offset + ft[3:6])
normalt = (np.array(p2t) - np.array(p1))
normalt = normalt / np.linalg.norm(normalt)
d = DebugData()
# force
if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1:
d.addLine(p1, p2f, color=[1.0, 0.0, 0.0])
else:
d.addArrow(p1, p2f, color=[1.,0.,0.])
# torque
if self.draw_torque:
d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6]))
# frame (largely for debug)
vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2)
vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')
def doFTDraw(self, unusedrobotstate):
frames = []
fts = []
vis_names = []
if (hasattr(self.robotStateJointController, 'lastRobotStateMessage')
and self.robotStateJointController.lastRobotStateMessage):
if self.draw_right:
rft = np.array(
self.robotStateJointController.lastRobotStateMessage.
force_torque.r_hand_force + self.robotStateJointController.
lastRobotStateMessage.force_torque.r_hand_torque)
rft[0] = -1. * rft[
0] # right FT looks to be rotated 180deg around the z axis
rft[1] = -1. * rft[1]
rft[3] = -1. * rft[3]
rft[4] = -1. * rft[4]
rft -= self.ft_right_bias
fts.append(rft)
frames.append(
self.robotStateModel.getLinkFrame('r_hand_force_torque'))
vis_names.append('ft_right')
if self.draw_left:
lft = np.array(
self.robotStateJointController.lastRobotStateMessage.
force_torque.l_hand_force + self.robotStateJointController.
lastRobotStateMessage.force_torque.l_hand_torque)
lft -= self.ft_left_bias
fts.append(lft)
frames.append(
self.robotStateModel.getLinkFrame('l_hand_force_torque'))
vis_names.append('ft_left')
for i in range(0, len(frames)):
frame = frames[i]
ft = fts[i]
offset = [0., 0., 0.]
p1 = frame.TransformPoint(offset)
scale = 1.0 / 25.0 # todo add slider for this?
scalet = 1.0 / 2.5
p2f = frame.TransformPoint(offset + ft[0:3] * scale)
p2t = frame.TransformPoint(offset + ft[3:6])
normalt = (np.array(p2t) - np.array(p1))
normalt = normalt / np.linalg.norm(normalt)
d = DebugData()
# force
if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1:
d.addLine(p1, p2f, color=[1.0, 0.0, 0.0])
else:
d.addArrow(p1, p2f, color=[1., 0., 0.])
# torque
if self.draw_torque:
d.addCircle(p1, normalt, scalet * np.linalg.norm(ft[3:6]))
# frame (largely for debug)
vis.updateFrame(frame,
vis_names[i] + 'frame',
view=self.view,
parent='wristft',
visible=False,
scale=0.2)
vis.updatePolyData(d.getPolyData(),
vis_names[i],
view=self.view,
parent='wristft')
