def from_yaml(cls, array_yaml):
offset_quaternion = numpy.array(array_yaml['offset']['orientation'],
dtype='float')
offset_pose = openravepy.matrixFromQuat(offset_quaternion)
offset_pose[0:3, 3] = numpy.array(array_yaml['offset']['position'],
dtype='float')
return cls(offset_pose, array_yaml['origin'], array_yaml['normal'])
def rotation_interpolation(r1, r2, t):
quat1 = rave.quatFromRotationMatrix(r1)
quat2 = rave.quatFromRotationMatrix(r2)
# print(r1)
# print(r2)
t = min(max(t, 0), 1)
quat_interpolate = rave.quatSlerp(quat1, quat2, t, True)
return rave.matrixFromQuat(quat_interpolate)[0:3, 0:3]
def AddGaussianNoise(self, orientationSigma, positionSigma):
'''Returns a new grasp with noise added to the current position and orientation.'''
descriptor = self if rand() < 0.5 else self.Flip()
q = openravepy.quatFromRotationMatrix(descriptor.T)
q += orientationSigma * randn(4)
q /= norm(q)
T = openravepy.matrixFromQuat(q)
c = descriptor.center + positionSigma * descriptor.center
T[0:3, 3] = c
return HandDescriptor(\
T, image=None, function=descriptor.function, objectName=descriptor.objectName)
def info_callback(self, info):
with self.lock:
self.camera_info = info;
if (self.running and not (self.camera is None)):
try:
(trans, rot) = self.listener.lookupTransform(self.fixed_frame, self.camera_info.header.frame_id,
self.camera_info.header.stamp);
m = openravepy.matrixFromQuat(np.array([rot[3], rot[0], rot[1], rot[2]]));
m[0, 3] = trans[0];
m[1, 3] = trans[1];
m[2, 3] = trans[2];
self.camera.SetTransform(m);
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
#print "Failed to lookup " + self.camera_info.header.frame_id + " in " + self.fixed_frame;
pass;
def PlaceObjectSet(self, objectNames, objectScales):
'''TODO'''
# parameters
positionMean = array([0, 0, 0.03])
positionSigma = array([0.08, 0.08, 0.02])
with self.env:
# load objects into environment
objectHandles = []
for i in xrange(len(objectNames)):
self.env.Load(objectNames[i],
{'scalegeometry': str(objectScales[i])})
idx1 = objectNames[i].rfind("/")
idx2 = objectNames[i].rfind(".")
shortObjectName = objectNames[i][idx1 + 1:idx2]
objectHandle = self.env.GetKinBody(shortObjectName)
objectHandle.SetName(shortObjectName + "-" + str(i))
objectHandles.append(objectHandle)
# place objects randomly
objectPoses = []
for i, objectHandle in enumerate(objectHandles):
# sample orientation
q = randn(4)
q = q / norm(q)
objectPose = openravepy.matrixFromQuat(q)
# sample position
position = positionMean + positionSigma * randn(3)
objectPose[0:3, 3] = position
# set object transform
objectHandle.SetTransform(objectPose)
objectPoses.append(objectPose)
return objectHandles, objectPoses
def from_yaml(cls, array_yaml):
offset_quaternion = numpy.array(array_yaml["offset"]["orientation"], dtype="float")
offset_pose = openravepy.matrixFromQuat(offset_quaternion)
offset_pose[0:3, 3] = numpy.array(array_yaml["offset"]["position"], dtype="float")
return cls(offset_pose, array_yaml["origin"], array_yaml["normal"])
def deserialize_transform(data):
from openravepy import matrixFromQuat
t = matrixFromQuat(data['orientation'])
t[0:3, 3] = data['position']
return t
def trans_from_quat(quat):
return matrixFromQuat(quat)
def trans_from_quat(quat): return matrixFromQuat(quat)
