def place_rail(self, rail_place):
""" Place both rails in the environment
Args:
rail_place: (RailPlace) rail object.
"""
# P - primary rail,
# S - secondary rail,
# alpha - angle from the perpendicular to the primary rail
# Using camera standard axis and transformLookat for rails
# Primary Rail
primary_extent = self.primary.GetLinks()[0].GetGeometries(
)[0].GetBoxExtents()
primary_offset = array([
0, -primary_extent[1], -primary_extent[2]
]) + array([0, 0, self.config.environment.primary_safe_margin])
primary_offset_transform = eye(4)
primary_offset_transform[0:3, 3] = primary_offset
# Secondary Rail
secondary_extent = self.secondary.GetLinks()[0].GetGeometries(
)[0].GetBoxExtents()
# Resizing
secondary_extent[2] = abs(
rail_place.s) / 2.0 + self.config.environment.secondary_safe_margin
self.env.Remove(self.secondary)
self.secondary.InitFromBoxes(
array([concatenate([zeros(3), secondary_extent])]), True)
self.env.AddKinBody(self.secondary)
#
secondary_offset = array(
[0, -secondary_extent[1], secondary_extent[2]]) + array([
0, self.config.environment.robot_level_difference,
-self.config.environment.secondary_safe_margin
])
secondary_offset_transform = eye(4)
secondary_offset_transform[0:3, 3] = secondary_offset
# Rails Traonsform and Placement
primary_transform = transformLookat(
array([0, 0, self.config.environment.z_floor_level]),
array([rail_place.p, 0, self.config.environment.z_floor_level]),
[0, 0, 1])
self.primary.SetTransform(
dot(primary_transform, primary_offset_transform))
secondary_transform = transformLookat(
rail_place.getXYZ(self.config),
array([rail_place.p, 0, self.config.environment.z_floor_level]),
[0, 0, 1])
self.secondary.SetTransform(
dot(secondary_transform, secondary_offset_transform))
def sequence_goals(self):
if not len(self.goal_array):
self.goal = [None]
return
goals = numpy.array(deepcopy(self.goal_array))
tasks_msg = PoseArray()
for goal in goals:
T = openravepy.transformLookat(
goal + [0.1, 0.0, 0.0],
goal - [self.starting_position_offset, 0.0, 0.0], [0, 0, -1])
T = numpy.dot(T, numpy.linalg.inv(self.T_G2EE))
pose = openravepy.poseFromMatrix(T)
pose_msg = Pose()
pose_msg.orientation.w = pose[0]
pose_msg.orientation.x = pose[1]
pose_msg.orientation.y = pose[2]
pose_msg.orientation.z = pose[3]
pose_msg.position.x = pose[4]
pose_msg.position.y = pose[5]
pose_msg.position.z = pose[6]
tasks_msg.poses.append(pose_msg)
resp = self.sequencer_client.call(tasks_msg, SEQUENCING_TYPE)
self.sequenced_goals = [goals[i] for i in resp.sequence]
self.sequenced_trajectories = resp.database_trajectories
self.num_goals_history = len(self.sequenced_goals)
print("sequenced goals: " + str(self.sequenced_goals))
print("resp.sequence: " + str(resp.sequence))
if self.sim:
self.goal = numpy.array(self.sequenced_goals[0])
self.goal_off = self.goal - self.go_to_goal_offset * self.normalize(
self.goal - self.ee_position)
self.starting_position = self.goal - numpy.array(
[self.starting_position_offset, 0.0, 0.0])
self.starting_direction = numpy.array([1.0, 0.0, 0.0])
def palmDistanceMetric(self):
self.SIG.loadObject('cube', 9, 6, 3)
self.SIG.Obj.changeColor('purpleI')
HandT = np.array([[0.08, 1., -0.03, 0.], [1., -0.08, -0., -0.],
[-0., -0.03, -1., 0.21], [0., 0., 0., 1.]])
cameraT = np.array([[0.02598803, 0.99090335, 0.13204235, -0.08580729],
[0.09352606, 0.12909753, -0.98721158, 0.6148898],
[-0.9952776, 0.03800508, -0.0893203, 0.09105067],
[0., 0., 0., 1.]])
filename = os.path.join(self.ImageFolder, 'PalmDistanceMetric')
self.HandList[0].obj.SetTransform(HandT)
self.SIG.groundPlane.createGroundPlane(self.SIG.obj.h / 2 / 100.0)
self.SIG.vis.viewer.SetCamera(
transformLookat(
[0, 0, 0], [0, -.6, 0],
[1, 0, 0])) # move point to between palm and face of object
self.HandList[0].changeColor(color='yellowI')
# self.SIG.vis.viewer.SetCamera(np.round(cameraT,4))
self.SIG.vis.takeImage(filename)
pdb.set_trace()
def camera_look_at(point, look_point=unit_point()):
return transformLookat(
np.array(look_point) - np.array(point), np.array(point), -unit_z())
def camera_look_at(point, look_point=unit_point()): return transformLookat(np.array(look_point)-np.array(point), np.array(point), -unit_z())