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)