def Pose(self):
o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6))
if o != 0:
x = self._tab.Indirect(o + self._tab.Pos)
from Pose import Pose
obj = Pose()
obj.Init(self._tab.Bytes, x)
return obj
return None
def draw(points, func_dict):
func_dict["pen"](False)
move_forward(Pose(x=points[0].x, y=points[0].y), 1, 1, 0.1, func_dict)
func_dict["pen"](True)
for point in points[1:]:
move_forward(Pose(x=point.x, y=point.y), 1, 1, 0.1, func_dict)
func_dict["log"]("Finished!")
def __calcluateTurn(self, tacho):
pose = Pose()
# the radian during this turn
deltaTheta = self.__calcluateRadianFromMotorTacho(tacho)
self.pose.theta -= deltaTheta
pose.x = self.pose.x
pose.y = self.pose.y
pose.theta = self.pose.theta%(math.pi*2)
return pose
def main( args ):
wts = Weights( args.density_score_wt, args.overlap_score_wt, args.closab_score_wt, args.clash_score_wt )
scorefxn = ScoreFunction( args.density_scorefile, args.overlap_scorefile, args.nonoverlap_scorefile, wts, args.null_frag_score )
for model in range( args.nstruct ):
pose = Pose() # empty pose
pose.initialization( scorefxn._density_score_dict, args.initialization ) # default initialize by random
pose.show_state( "initialization" )
temp = args.starting_temperature
steps = args.steps_per_temp
mc = MonteCarlo( scorefxn, temp )
trajectory_tracker_dict = {}
while temp>=0 or anneal_temp==0: # it would never reach this criteria
run_tag = "model_" + str( model ) + "_temp_" + str( temp )
mc.apply( pose, steps )
pose.show_state( run_tag )
#pose.dump_pickle( run_tag + ".pickle" )
trajectory_tracker_dict[ run_tag ] = pose
anneal_temp = round( temp*args.cooling_rate, 1 )
temp -= anneal_temp
if temp == mc.temperature(): break
mc.set_temperature( temp )
pickle.dump( trajectory_tracker_dict, open( "model_" + str( model ) + ".pickle", "w" ) )
pose.show_state( "model_" + str( model ) + "_end", True ) # True for verbose
def lowest_score_pose( self ):
''' return the lowest total score pose '''
# scoreterm in pose
lowest_score = 100000000000
lowest_score_pose = Pose()
for pose in self:
if pose._total_score < lowest_score:
lowest_score = pose._total_score
lowest_score_pose = pose
lowest_score_pose = lowest_score_pose.clone()
return lowest_score_pose
def makeBeaker():
pc = Pose(0, 0, 0, 0, 0, 0)
pcy = Pose(0, 0, 1, 0, 0, 0)
cone = Cone(pc, 1.5, .25)
cylinder = Cylinder(pcy, .5, .1)
axes = cone.combinePrimatives([cylinder])
graspCone = cone.planGrasps([1, 1, 1, 1])
graspCylinder = cylinder.planGrasps([1, 1, 1, 1])
cone.visualizeGrasps(axes, graspCone)
cylinder.visualizeGrasps(axes, graspCylinder)
plt.show()
def makeCoffeeMug():
pc = Pose(0, 0, 0, 0, 0, 0)
pb = Pose(.6, 0, 0, 0, 0, 0)
box = Box(pb, .75, .2, .5)
cylinder = Cylinder(pc, 1, 1)
axes = cylinder.combinePrimatives([box])
graspBox = box.planGrasps([1, 5, 5, 1])
graspCylinder = cylinder.planGrasps([5, 5, 5, 5])
box.visualizeGrasps(axes, graspBox)
cylinder.visualizeGrasps(axes, graspCylinder)
plt.show()
def makePlane():
pb1 = Pose(0, 0, 0, 0, 0, 0)
pb2 = Pose(0, 0, 0, 0, 0, 0)
box1 = Box(pb1, .3, 0.25, 3)
box2 = Box(pb2, .15, 3, .6)
axes = box1.combinePrimatives([box2])
graspBox1 = box1.planGrasps([1, 1, 1, 1])
graspBox2 = box2.planGrasps([1, 1, 1, 1])
box1.visualizeGrasps(axes, graspBox1)
box2.visualizeGrasps(axes, graspBox2)
plt.show()
def callback(data):
ori = data.pose.orientation
Euler = quatToEuler([ori.x, ori.y, ori.z, ori.w])
p0 = Pose(data.pose.position.x, data.pose.position.y, data.pose.position.z,
Euler[0], Euler[1], Euler[2])
param = data.parameters
switch[data.shapetype.data](p0, param)
def __init__(self, wheelRadius, wheelBaseLength, ticksPerRev, minRPM,
maxRPM, minVel, maxVel):
self.wheelRadius = wheelRadius # En metros.
self.wheelBaseLength = wheelBaseLength # En metros.
self.ticksPerRev = ticksPerRev #Entero
self.minRPM = minRPM
self.maxRPM = maxRPM
#self.minVel = minRPM * 2 * pi / 60 # En rad/s
#self.maxVel = maxRPM * 2 * pi / 60 # En rad/s
self.minVel = 0 #0.150006681210744#0#3.2498914648863666#0.0394496029208 # En rad/s
self.maxVel = 1.0262706579357654 #5.150818988820947#0.0433583709166 # En rad/s
self.beta = [0.0050072227207064515, -0.25057113358444087]
self.prevTicks = ticks(0, 0)
self.rightWheelSpeed = 0
self.leftWheelSpeed = 0
self.stateEstimate = Pose(x=0, y=0, theta=0, yawO=0)
self.dynamics = DifferentialDrive(self.wheelRadius,
self.wheelBaseLength)
self.pi = pigpio.pi()
self.rightMotor = None
self.leftMotor = None
self.leftEnconder = None
self.rightEncoder = None
self.imu = None
self.ultrasonics = []
self.usLocation = [[0.15, 0.21, 90], [0.29, 0, 0], [0.15, -0.21, -90]]
def get_rescaled_points(self, prediction, frame, threshold=0.1):
frame_width = frame.shape[1]
frame_height = frame.shape[0]
height = prediction.shape[2]
width = prediction.shape[3]
# Empty list to store the detected keypoints
points = []
for i in range(len(self.pose_pairs)):
# confidence map of corresponding body's part.
prob_map = prediction[0, i, :, :]
if i == 1:
plt.imshow(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB))
plt.imshow(prob_map, alpha=0.6)
# Find global maxima of the probMap.
_, prob, _, point = cv2.minMaxLoc(prob_map)
# Scale the point to fit on the original image
x = (frame_width * point[0]) / width
y = (frame_height * point[1]) / height
if prob > threshold:
# Add the point to the list if the probability is greater than the threshold
points.append((int(x), int(y)))
else:
points.append(None)
return Pose(points)
def makePhone():
pb1 = Pose(0, 0, 0, 0, 0, 0)
pb2 = Pose(.5, 0, 1.5, 0, 0, 0)
pb3 = Pose(.5, 0, -1.5, 0, 0, 0)
box1 = Box(pb1, 4, .75, .5)
box2 = Box(pb2, 1, .75, .5)
box3 = Box(pb3, 1, .75, .5)
axes = box1.combinePrimatives([box2, box3])
graspBox1 = box1.planGrasps([1, 1, 1, 1])
graspBox2 = box2.planGrasps([1, 1, 1, 1])
graspBox3 = box3.planGrasps([1, 1, 1, 1])
box1.visualizeGrasps(axes, graspBox1)
box2.visualizeGrasps(axes, graspBox2)
box3.visualizeGrasps(axes, graspBox3)
plt.show()
def __init__(self, id, pose=None, defaultPose=False, collisionRadius=0.15):
assert defaultPose == False or isinstance(defaultPose, Pose), ERR.TYPE_MISMATCH(defaultPose, Pose)
self.type = "AGENT"
self.id = id
self.defaultPose = defaultPose if defaultPose else copy.deepcopy(pose)
self.collisionRadius= collisionRadius
if pose == None:
pose = Pose()
self.reset(pose)
def simulate_trajectory(self, robot, vx, vtheta):
poses = []
current_pose = [robot.pose.x, robot.pose.y, robot.pose.theta]
for i in range(0, int(self.time / self.timestep)):
current_pose[0] += (vx * math.cos(current_pose[2])) * self.timestep
current_pose[1] += (vx * math.sin(current_pose[2])) * self.timestep
current_pose[2] += vtheta * self.timestep
new_pose = Pose(current_pose[0], current_pose[1], current_pose[2])
poses.append(new_pose)
return Trajectory(poses=poses, velocity=Velocity(vx, vtheta), cost=0)
def visualizeGrasps(self, ax, grasps):
"""
adds the grasp vectors to the plot
each grasp will plot their x and z vector
:param ax: axes object
:return:
"""
numGrasps = len(grasps)
poses = (g.pose for g in grasps) # get list of poses of grasps
# quiver list consisting of: x, y, z, u, v, w, color
numAxis = 2
q = np.zeros([numAxis * numGrasps, 6])
c = np.zeros([numAxis * numGrasps, 6])
for i, p in enumerate(poses):
T = Pose.makeTranformfromPose(p)
# add z axis to plot
Tz = T[0:3, 2]
q[i * numAxis] = np.array([p.x, p.y, p.z, Tz[0], Tz[1], Tz[2]])
c[i * numAxis] = (0)
# add z axis to plot
if numAxis >= 2:
Tx = T[0:3, 0]
q[i * numAxis + 1] = np.array(
[p.x, p.y, p.z, Tx[0], Tx[1], Tx[2]])
c[i * numAxis + 1] = (.33)
# add y axis to plot
if numAxis >= 3:
Ty = T[0:3, 1]
q[i * numAxis + 2] = np.array(
[p.x, p.y, p.z, Ty[0], Ty[1], Ty[2]])
c[i * numAxis + 2] = (.66)
# norm = plt.Normalize()
# norm.autoscale(c)
# colormap = plt.colormaps()
# cm = plt.cm.jet(c)
# print(colormap[c])
# print(cm)
ax.quiver(q[:, 0],
q[:, 1],
q[:, 2],
q[:, 3],
q[:, 4],
q[:, 5],
color='r',
length=0.5)
# axis('equal')
return
def _randomPoseSamples(self):
poseSamples = []
for n in range(self.numPoses):
poseSamples.append(
Pose({
'rot': [
np.random.randint(-180, 180),
np.random.randint(-20, 40), 0
]
}))
return poseSamples
def __init__( self, scorefxn, steps, temp, cooling_rate ):
''' '''
assert isScoreFxn( scorefxn )
self._log = ""
self._scorefxn = scorefxn
self._steps = steps # MC steps per annealing cycle
self._temperature = temp
# declare objects
self.__pose = Pose()
self._initialization = "random"
self._cooling_rate = cooling_rate
self._anneal_temp = 0.0
# recover_low ( quench )
self._quench = False
self._lowest_score_pose = Pose()
self._record_trajectory = False
def getEndGrasps(self, graspParams, surfaceOffset=0.1):
"""
Generate the grasps for grabbing a cone from the bottom or top end
:param graspParams:[array 1x4] Grasp parameters associated with creating grasps
:param surfaceOffset:[double] Distance to start the grasp away from the surface
:return: [list<Grasp>] A list of grasps
"""
graspRotations = int(graspParams[2])
graspList = []
for i in range(2):
# rotation about the y axis only if i == 1
rotYMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, i * np.pi, 0))
interMatrix = np.matmul(Pose.makeTranformfromPose(self.originPose),
rotYMatrix)
# translate along negative Z axis to bring the grasp out of the object
transZMatrix = Pose.makeTranformfromPose(
Pose(0, 0,
-abs(i * self.height - self.height / 4) - surfaceOffset,
0, 0, 0))
endMatrix = np.matmul(interMatrix, transZMatrix)
wristRotation = 0
for j in range(graspRotations):
# Rotate about the z axis for plan the set of grasps
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, wristRotation))
graspMatrix = np.matmul(endMatrix, rotZMatrix)
# increment rotation for next loop
wristRotation += 2 * np.pi / graspRotations
# Add Grasp to list
graspList.append(
Grasp('spherical',
Pose.makePoseFromTransform(graspMatrix)))
return graspList
def main( args ):
print print_args( args )
wts = Weights( args.density_score_wt, args.overlap_score_wt, args.closab_score_wt, args.clash_score_wt )
scorefxn = ScoreFunction( args.density_scorefile, args.overlap_scorefile, args.nonoverlap_scorefile, wts, args.null_frag_score )
pose = Pose() # empty pose
pose.initialization( scorefxn._density_score_dict ) # default initialize by random
temp = args.temperature
mc = MonteCarlo( scorefxn, temp )
for each_round in range( 1, args.round+1 ):
mc.apply( pose, args.steps )
pose.show_state( temp )
temp -= round( temp*0.1, 2 )
if temp <= 0: break
mc.set_temperature( temp )
pose.dump_pickle()
def __calcluateMove(self, tacho):
pose = Pose()
moveDistance = self.__calcluateMoveDistanceFromTacho(tacho)
pose.x, pose.y = self.__getCoord(self.pose.x, self.pose.y, self.pose.theta, moveDistance)
pose.theta = self.pose.theta
return pose
pose["L_Hip_Roll"] = HR_C+HR_A*math.sin(2*math.pi*simTime/T+HR_Phi)
pose["R_Hip_Roll"] = HR_C+HR_A*math.sin(2*math.pi*simTime/T+HR_Phi+math.pi)
#Hip Pitch
pose["L_Hip_Pitch"] = HP_C+HP_A*math.sin(2*math.pi*simTime/T+HP_Phi)
pose["R_Hip_Pitch"] = HP_C+HP_A*math.sin(2*math.pi*simTime/T+HP_Phi+math.pi)
#Knee Pitch
pose["L_Knee"] = K_C+K_A*math.sin(2*math.pi*simTime/T+K_Phi)
pose["R_Knee"] = RK_Pos=K_C+K_A*math.sin(2*math.pi*simTime/T+K_Phi+math.pi)
#Ankle Pitch
pose["L_Ankle_Pitch"] = AP_C+AP_A*math.sin(2*math.pi*simTime/T+AP_Phi)
pose["R_Ankle_Pitch"] = AP_C+AP_A*math.sin(2*math.pi*simTime/T+AP_Phi+math.pi)
#Ankle Roll
pose["L_Ankle_Roll"] = AR_C+AR_A*math.sin(2*math.pi*simTime/T+AR_Phi)
pose["R_Ankle_Roll"] = AR_C+AR_A*math.sin(2*math.pi*simTime/T+AR_Phi+math.pi)
newPose = Pose(pose)
lucy.executePose(newPose)
poseNumber = poseNumber + 1
for joint in configuration.getJointsName():
xmlJoint = ET.SubElement(frame, joint)
joint_id = configuration.loadJointId(joint)
pos = newPose.getValue(joint)
degreeAngle = 150-(pos*float(60))
xmlJointAngle = xmlJoint.set("angle" , str(degreeAngle))
counter = counter + 1
lucy.stopLucy()
tree = ET.ElementTree(root)
tree.write("moon_walk1.xml")
print 'Program ended'
def testCylinderGraspLocations():
cylinder = Cylinder(Pose(0, 0, 5, 0, 0, 0), 10, 1)
grasps = cylinder.planGrasps([1, 2, 1, 1])
print(grasps)
def __init__(self, x, y, theta, current_vel_x, current_vel_theta,
max_accel_x, max_accel_theta):
self.pose = Pose(x, y, theta)
self.velocity = Velocity(current_vel_x, current_vel_theta)
self.max_accel = (max_accel_x, max_accel_theta)
def getSideGrasps(self, graspParams, surfaceOffset=0.1):
"""
Returns the Side Grasps for a cylinder object
:param surfaceOffset: [double] Distance to start the grasp away from the surface
:param graspParams: [array 1x4] Grasp parameters associated with creating grasps
:return: [list<Grasp>] A list of grasps
"""
parallelPlanes = int(graspParams[0]) # this number should be odd
divisionsOf360 = int(graspParams[1])
grasp180Rotations = int(graspParams[3])
graspList = []
# Side Grasps
if parallelPlanes > 1: # set the initial height at the bottom of cylinder
graspTranslation = -self.height / 2
else: # set initial height at center of mass
graspTranslation = 0
for i in range(parallelPlanes):
# Translation along the objects central axis (z axis) for height placement of grasp
translationMatrix = Pose.makeTranformfromPose(
Pose(0, 0, graspTranslation, 0, 0, 0))
parallelMatrix = np.matmul(
Pose.makeTranformfromPose(self.originPose), translationMatrix)
print(i)
# increment GraspTranslation and set grasp rotation
if parallelPlanes > 1: # don't divide by 0
graspTranslation += self.height / (parallelPlanes - 1)
graspRotation = 0
for j in range(divisionsOf360):
# Rotation about the objects central axis (z axis) for rotation placement of grasp
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, graspRotation))
divisionsMatrix = np.matmul(parallelMatrix, rotZMatrix)
# increment grasp rotation and set wrist rotation
graspRotation += 2 * np.pi / divisionsOf360
wristRotation = 0
for k in range(2 * grasp180Rotations):
# These three transformation matrices move the frame of the grasp outside the object.
# Additional it guarantees that the z axis of the frame points towards the object (approach vector)
# and that the x axis is perpendicular to the central axis and the approach vector to achieve the
# thumb vector
transXMatrix = Pose.makeTranformfromPose(
Pose(self.radius + surfaceOffset, 0, 0, 0, 0, 0))
rotYMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, -np.pi / 2, 0))
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, -np.pi / 2))
# multiplication of the transformation matrices
graspMatrix = np.matmul(divisionsMatrix, transXMatrix)
graspMatrix = np.matmul(graspMatrix, rotYMatrix)
graspMatrix = np.matmul(graspMatrix, rotZMatrix)
# Rotate about the approach vector (our z axis) if we need to
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, wristRotation))
graspMatrix = np.matmul(graspMatrix, rotZMatrix)
wristRotation += np.pi / 2
# Add object to grasp list as a Pose not a matrix
graspList.append(
Grasp('cylindrical',
Pose.makePoseFromTransform(graspMatrix)))
return graspList
self._density_score_dict[ pos ][ frag_id ] = ( densityScore, rmsd )
else:
self._density_score_dict[ pos ] = { frag_id : ( densityScore, rmsd ) }
self._density_score_dict[ pos ][ ( mer, pos, 0, 0 ) ] = ( 0, 0 ) # for each position, create a Null fragment fake density score, because you will need to print out density score when you select a null frag for a position
pickle.dump( self._density_score_dict, open("density_score_Dict.pickle", "w") )
stdout.write("done\n")
if __name__=="__main__":
parser = ArgumentParser()
parser.add_argument("-d", "--density_scorefile", required=True, help="")
parser.add_argument("-t", "--rmsd_threshold", default=2.5, type=float, help="")
args = parser.parse_args()
dens = DensityScore( args.density_scorefile )
dict = dens._density_score_dict
pose = Pose()
pose.initialization( dict, "lowrmsd" )
print pose.total_score()
#for pos in dens._density_score_dict.keys():
# selected_frag = (9,pos,0,0)
# for frag in dens._density_score_dict[ pos ]:
# rmsd = dens._density_score_dict[ pos ][ frag ][1]
# if rmsd < args.rmsd_threshold and rmsd != 0:
# selected_frag = frag
# try:
# print "after_rotation_frags.%s.????.pdb" %(".".join( map( str, selected_frag ) )), dens._density_score_dict[ pos ][ selected_frag ]
# except:
# continue
# you might need to change it in the future
parser.add_argument("--mer", default=9, type=int, help="")
parser.add_argument("--verbose", default=False, action="store_true", help="")
args = parser.parse_args()
print print_args(args)
Wts = Weights(args.density_score_wt, args.overlap_score_wt, args.closab_score_wt, args.clash_score_wt)
Scorefxn = ScoreFunction(
args.density_scorefile, args.overlap_scorefile, args.nonoverlap_scorefile, Wts, args.null_frag_score
)
for each_round in range(1, args.round + 1):
Pose = Pose()
Pose.initialization(Scorefxn.density_score_dict) # default initialize by random
Scorefxn.update_pose(Pose)
working_temp = args.temperature
for each_step in range(args.steps):
print "round: %s cycle: %s" % (each_round, each_step)
pos = random.sample(Pose.dict.keys(), 1)[0] # pick a residue number to start with
null_frag_id = (args.mer, pos, 0, 0)
Boltzmann = PerRsdBoltzmann(working_temp)
""" After picking a position to optimize, calculate compatibility scores for all the candidate placements at that residue """
def __init__(self, pose=Pose(), h=0, w=0, l=0):
Shape.__init__(self, pose)
self.height = h
self.width = w
self.length = l
def __init__(self, originPose=Pose()):
self.originPose = originPose # position of centroid of the object with orientation
self.transformation = Pose.makeTranformfromPose(originPose)
# params
self.grasps = []
class SimulatedAnnealing:
'''
1. easy to dump the object as a resulting model, and the trajectory?
2. linear cooling rate
'''
def __init__( self, scorefxn, steps, temp, cooling_rate ):
''' '''
assert isScoreFxn( scorefxn )
self._log = ""
self._scorefxn = scorefxn
self._steps = steps # MC steps per annealing cycle
self._temperature = temp
# declare objects
self.__pose = Pose()
self._initialization = "random"
self._cooling_rate = cooling_rate
self._anneal_temp = 0.0
# recover_low ( quench )
self._quench = False
self._lowest_score_pose = Pose()
self._record_trajectory = False
def set_annealing_temperature( self ):
''' '''
self._anneal_temp = round( self._temperature*self._cooling_rate, 1 )
self._temperature -= self._anneal_temp
def run( self, runid ):
''' run id is a identification for a model to dump '''
self.__pose.clear()
self.__pose.initialization( self._scorefxn.get_density_score_dict(), self._initialization ) # default initialize by random
print self.__pose.show_state( "initialization" )
mc = MonteCarlo( self._scorefxn, self._temperature )
mc.apply( self.__pose, self._steps ) # to prevent a silly bug that at the very first state SCORE==0, such that you wouldn't lower than that during high temperatures sampling
self._lowest_score_pose = self.__pose.clone()
tracker = TrajectoryTracker( runid )
tag = "model_" + str( runid )
while self._temperature >=0 : # it would never reach this criteria
if self._quench:
self.recover_low() # retrieve the lowest-score pose from previous runs
mc.apply( self.__pose, self._steps )
print self.__pose.show_state( tag + "_" + str( self._temperature ) )
self.set_annealing_temperature() # update self._temperature
if self._temperature == mc.get_temperature(): break
mc.set_temperature( self._temperature )
if self._record_trajectory:
tracker.save( self._temperature, self.__pose )
tracker.save( self._temperature, self.__pose )
tracker.dump_pickle( tag )
self.__pose.show_state( tag + "_final", True ) # True for verbose showing all residues states
def set_to_quench( self ):
self._quench = True
def record_trajectory( self ):
self._record_trajectory = True
def recover_low( self ): # recover_low
'''
last accept score pose, and lowest score pose; the MC.recover_low has this is because it uses Metropolis to thermally accept pose
'''
# this is for the first around
# need to think about a better way to initiate it
if not self._lowest_score_pose._initialized:
self._lowest_score_pose._initialized = self.__pose
# .total_score() method will re-evaluate pose whenever it is called
if self.__pose.total_score() > self._lowest_score_pose.total_score():
stdout.write("recover previous found lowest-score pose with score: %s\n" % self._lowest_score_pose.total_score() )
self.__pose = self._lowest_score_pose.clone()
else:
stdout.write("found a newer lowest-score pose\n")
self._lowest_score_pose = self.__pose.clone()
def dump( self, id ):
return
def quench( self ):
return
def __init__(self, pose=Pose(), height=0, radius=0):
Shape.__init__(self, pose)
self.height = height
self.radius = radius
def main():
isrec = 0
# muitmap=0
tello = Tello()
pydisplay = Pydisplay()
keyuser = Keyuser() # 键盘命令
ui = UID()
mapui = Mapui()
# if muitmap==0:
# mapui=Mapui()
# else:
# mapstack,mapsd = Pipe()
# #stack= Manager().list()
# mapshowing = Process(target=mapread, args=(mapstack,))
# mapshowing.start()
pose = Pose()
com = Com()
mapcom = Mapcom()
frame_skip = 300
# pidtuning
# if mapcom.tpid==1:
# pidimg=np.zeros((500, 512, 3), np.uint8)
# cv2.namedWindow('pidyaw')
# cv2.namedWindow('pidthro')
# cv2.namedWindow('pidpith')
# cv2.namedWindow('pidroll')
# cv2.createTrackbar('p', 'pidyaw', 0, 100, nothing)
# cv2.createTrackbar('i', 'pidyaw', 0, 100, nothing)
# cv2.createTrackbar('d', 'pidyaw', 0, 100, nothing)
# cv2.createTrackbar('down', 'pidyaw', 0, 100, nothing)
# cv2.createTrackbar('up', 'pidyaw', 0,100, nothing)
# cv2.createTrackbar('p', 'pidthro', 0, 100, nothing)
# cv2.createTrackbar('i', 'pidthro', 0, 100, nothing)
# cv2.createTrackbar('d', 'pidthro', 0, 100, nothing)
# cv2.createTrackbar('down', 'pidthro', 0, 100, nothing)
# cv2.createTrackbar('up', 'pidthro', 0,100, nothing)
# cv2.createTrackbar('p', 'pidpith', 0, 100, nothing)
# cv2.createTrackbar('i', 'pidpith', 0, 100, nothing)
# cv2.createTrackbar('d', 'pidpith', 0, 100, nothing)
# cv2.createTrackbar('down', 'pidpith', 0, 100, nothing)
# cv2.createTrackbar('up', 'pidpith', 0,100, nothing)
# cv2.createTrackbar('p', 'pidroll', 0, 100, nothing)
# cv2.createTrackbar('i', 'pidroll', 0, 100, nothing)
# cv2.createTrackbar('d', 'pidroll', 0, 100, nothing)
# cv2.createTrackbar('down', 'pidroll', 0, 100, nothing)
# cv2.createTrackbar('up', 'pidroll', 0,100, nothing)
# 录像功能
if isrec:
stack, sd = Pipe()
#stack= Manager().list()
pr = Process(target=read, args=(stack, ))
pr.start()
# try:
for frame in tello.container.decode(video=0): # 一定要用这个循环来获取才不会产生delay
if 0 < frame_skip:
frame_skip = frame_skip - 1
continue
start_time = time.time()
image2surface = numpy.array(frame.to_image()) # 做个拷贝给pygame
image = cv2.cvtColor(image2surface, cv2.COLOR_RGB2BGR)
key_list = pygame.key.get_pressed()
imageraw = image
image = cv2.resize(image, (640, 480)) # 这个太大会爆显存
userc = keyuser.usec(key_list) # 来自用户输入的命令
# userc[0 1 2 3 4 5 ]
# 是否使用openpose 四个通道 模式
if userc[4] == 0 or userc[4] == 1:
if userc[4] == 1: # 判断使用跟踪
kp, out = pose.get_kp(image)
else: # 不使用
kp = [[0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0],
[0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0], [0, 0],
[0, 0], [0, 0], [0, 0], [0, 0], [0, 0]]
comd = com.get_comd(kp, userc) # 接受两个数组进行判断
tello.send_comd(comd)
flight = tello.send_data() # 飞行数据
com.read_tello_data(flight) # 飞控获取数据用于判断指令
flightstate = com.get_state() # 命令状态
if userc[4] == 1: # 使用
rec = ui.show(out, kp, flightstate) # 显示并负责播放声音
else: # 不用
rec = ui.show(imageraw, 0, flightstate)
if isrec:
write(sd, rec)
# print(userc[4])
elif userc[4] == 2 or userc[4] == 3:
# if mapcom.tpid==1:
# cv2.imshow('pidyaw', pidimg)
# cv2.imshow('pidthro', pidimg)
# cv2.imshow('pidpith', pidimg)
# cv2.imshow('pidroll', pidimg)
# pid=[[cv2.getTrackbarPos('p', 'pidyaw')/10,cv2.getTrackbarPos('i', 'pidyaw')/100,cv2.getTrackbarPos('d', 'pidyaw')/10,-cv2.getTrackbarPos('down', 'pidyaw'),cv2.getTrackbarPos('up', 'pidyaw')],
# [cv2.getTrackbarPos('p', 'pidthro')/10,cv2.getTrackbarPos('i', 'pidthro')/100,cv2.getTrackbarPos('d', 'pidthro')/10,-cv2.getTrackbarPos('down', 'pidthro'),cv2.getTrackbarPos('up', 'pidthro')],
# [cv2.getTrackbarPos('p', 'pidpith')/10,cv2.getTrackbarPos('i', 'pidpith')/100,cv2.getTrackbarPos('d', 'pidpith')/10,-cv2.getTrackbarPos('down', 'pidpith'),cv2.getTrackbarPos('up', 'pidpith')],
# [cv2.getTrackbarPos('p', 'pidroll')/10,cv2.getTrackbarPos('i', 'pidroll')/100,cv2.getTrackbarPos('d', 'pidroll')/10,-cv2.getTrackbarPos('down', 'pidroll'),cv2.getTrackbarPos('up', 'pidroll')]]
data = tello.send_data()
mapcom.readflightdata(data)
# if mapcom.tpid==1:
# comd=mapcom.com(userc,pid)
# else:
comd = mapcom.com(userc)
flightstate = mapcom.send_flightdata()
tello.send_comd(comd)
checkoutmap = mapcom.checkalldone()
if checkoutmap == 1:
userc[4] = 0
keyuser.us[4] = 0
mapcom.checkdone = None
if isrec:
write(sd, imageraw)
mapui.mapshow(flightstate)
# if muitmap==0:
# mapui.mapshow(flightstate)
# else:
# mapsand(mapsd,flightstate)
pydisplay.display(image2surface, flightstate) # pygame飞行界面
# 目前对丢帧策略的理解，只要分母不要小于飞机发送回来的最大帧速率则不会产生延迟同时保证帧率
# 例子里的60是不合理的，会多丢弃一半的帧，浪费辽
if frame.time_base < 1.0 / 35:
if userc[4] == 1:
time_base = 1.0 / 35 # 使用pose稍微保守一点
else:
time_base = 1.0 / 35
else:
time_base = frame.time_base
frame_skip = int((time.time() - start_time) / time_base)
k = cv2.waitKey(1) & 0xff # 与pygame的键盘存在未知冲突
if k == 27:
pygame.display.quit()
tello.drone.quit() # 退出
break
# print(time.time()-start_time)
# except:
# print('连接超时或发生错误退出辽')
cv2.destroyAllWindows() # 关掉飞机直接退出程序
tello.drone.quit()
pygame.display.quit()
from ventana import *
from matplotlib import pyplot as plt
from numpy.fft import fft
from Inertial import Inertial
from Velocity import Velocity
from Coordenadas import Coord
from Pose import Pose
SIZE_BEAM_METERS = 15 * 2
SIZE_BEAM_PIXELS = 908
SIZE_SCALE_FACTOR = 10
#bufferInertial = Inertial.ReadFromFile("../recursos/datos/sibiu-pro-carboneras-anforas-2.jdb.salida")
#bufferCoord = Coord.ReadFromFile("../recursos/datos/coordenadas.txt")
bufferCoord = Coord.ReadFromFile("../recursos/datos/datosposicion.txt")
bufferPose = Pose.ReadFromFile("../recursos/datos/pose.txt")
bufferVelocity = Velocity.ReadFromFile("../recursos/datos/velocity.txt")
cap = cv.VideoCapture('../recursos/datos/S200225_7.mp4')
mapa = np.zeros((4000, 4000), dtype="uint8")
mapa.fill(0) # or img[:] = 255
height = 1
def windowUI():
global window, app
app = QtWidgets.QApplication([])
window = Ventana()
window.show()
def _randomPoseSamples(self):
poseSamples = []
for n in range(self.numPoses):
poseSample = [np.random.randint(self.minPose[ix], self.maxPose[ix]) for ix in range(3)]
poseSamples.append(Pose({'rot':poseSample}))
return poseSamples
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
from simulator.SimLucy import SimLucy
from Pose import Pose
from simulator.AXAngle import AXAngle
from simulator.LoadRobotConfiguration import LoadRobotConfiguration
import xml.etree.cElementTree as ET
posesVect = {}
angle = AXAngle()
BalieroLucyMapping = {}
newPose = Pose()
robotConfig = LoadRobotConfiguration()
lucy = SimLucy(True)
root = ET.Element("root")
lucyPersistence = ET.SubElement(root, "Lucy")
configuration = LoadRobotConfiguration()
BalieroLucyMapping['L_Shoulder_Pitch']=2
BalieroLucyMapping['R_Shoulder_Pitch']=1
BalieroLucyMapping['L_Shoulder_Yaw']=4
BalieroLucyMapping['R_Shoulder_Yaw']=3
BalieroLucyMapping['L_Elbow_Yaw']=6
BalieroLucyMapping['R_Elbow_Yaw']=5
def getEdgeGrasps(self, graspParams, surfaceOffset=0.1):
"""
Generating grasps for grabbing a cone by its bottom edge
:param graspParams:[array 1x4] Grasp parameters associated with creating grasps
:param surfaceOffset:[double] Distance to start the grasp away from the surface
:return: [list<Grasp>] A list of grasps
"""
divisionsOf360 = int(graspParams[1])
grasp180Rotations = int(graspParams[3])
graspList = []
theta = math.atan(self.height / self.radius)
# hypotenuse of height and radius of cone
hypot = np.sqrt((self.height**2 + self.radius**2))
# translate along negative Z axis to bring the grasp out of the bottom surface of object
transZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, -(self.height / 4), 0, 0, 0))
endMatrix = np.matmul(Pose.makeTranformfromPose(self.originPose),
transZMatrix)
rotation = 0
for i in range(divisionsOf360):
# Rotation about the central axis of the object for divisions of 360
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, rotation))
divisionsMatrix = np.matmul(endMatrix, rotZMatrix)
# Translate to outer radius of cone
transXMatrix = Pose.makeTranformfromPose(
Pose(self.radius, 0, 0, 0, 0, 0))
edgeMatrix = np.matmul(divisionsMatrix, transXMatrix)
# Fix approach vector
rotYMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, -(np.pi / 2 - theta), 0))
edgeMatrix = np.matmul(edgeMatrix, rotYMatrix)
# Align x vector
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, -np.pi / 2))
edgeMatrix = np.matmul(edgeMatrix, rotZMatrix)
# move surface offset away from object
transZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, -surfaceOffset, 0, 0, 0))
edgeMatrix = np.matmul(edgeMatrix, transZMatrix)
rotation = rotation + 2 * np.pi / divisionsOf360
wristRotation = 0
for j in range(2 * grasp180Rotations):
# Rotate about the z axis for plan the set of grasps
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, wristRotation))
graspMatrix = np.matmul(edgeMatrix, rotZMatrix)
# increment rotation for next loop
wristRotation += np.pi / 2
# Add Grasp to list
graspList.append(
Grasp('cylindrical',
Pose.makePoseFromTransform(graspMatrix)))
return graspList
def getSideGrasps(self, graspParams, surfaceOffset=0.1):
"""
Generate the grasps for grabbing a cone from its side
:param graspParams:[array 1x4] Grasp parameters associated with creating grasps
:param surfaceOffset:[double] Distance to start the grasp away from the surface
:return: [list<Grasp>] A list of grasps
"""
parallelPlanes = int(graspParams[0]) # this number should be odd
divisionsOf360 = int(graspParams[1])
grasp180Rotations = int(graspParams[3])
graspList = []
theta = math.atan(self.height / self.radius)
# hypotenuse of height and radius of cone
hypot = np.sqrt((self.height**2 + self.radius**2))
# Side Grasp
rotation = 0
for i in range(divisionsOf360):
# Rotation about the central axis of the object for divisions of 360
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, rotation))
divisionsMatrix = np.matmul(
Pose.makeTranformfromPose(self.originPose), rotZMatrix)
rotation += 2 * np.pi / divisionsOf360
if parallelPlanes > 1: # divide hypotenuse by number of parallel planes and start at bottom of cone
planarTranslation = -hypot / 4 # this is because center of mass is 1/4 of the height not half
else:
planarTranslation = hypot / 4 # make sure the location doesn't move center of object hypotenuse
for j in range(parallelPlanes):
# First translate along the x axis to reach the slope
# center of mass is 1/4 of the height for a cone; using similar triangles to find distance to translate
transXMatrix = Pose.makeTranformfromPose(
Pose(3 / 4 * self.radius, 0, 0, 0, 0, 0))
slopeMatrix = np.matmul(divisionsMatrix, transXMatrix)
# Rotate about y axis by pi/2-theta to have the x axis perpendicular to slope
rotYMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, -(np.pi / 2 - theta), 0))
slopeMatrix = np.matmul(slopeMatrix, rotYMatrix)
# Align Z axis to be approach vector into cone
rotYMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, -np.pi / 2, 0))
slopeMatrix = np.matmul(slopeMatrix, rotYMatrix)
# Align X axis to be perpendicular to z axis and central axis
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, -np.pi / 2))
slopeMatrix = np.matmul(slopeMatrix, rotZMatrix)
# Have frame leave surface of object
transZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, -surfaceOffset, 0, 0, 0))
slopeMatrix = np.matmul(slopeMatrix, transZMatrix)
# Perform the parallel plane translation
transZMatrix = Pose.makeTranformfromPose(
Pose(0, planarTranslation, 0, 0, 0, 0))
parallelPlaneMatrix = np.matmul(slopeMatrix, transZMatrix)
if parallelPlanes > 1:
planarTranslation += hypot / (parallelPlanes - 1)
wristRotation = 0
for k in range(2 * grasp180Rotations):
# Rotate about the approach vector which should now be the z axis
rotZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, 0, 0, 0, wristRotation))
graspMatrix = np.matmul(parallelPlaneMatrix, rotZMatrix)
wristRotation += np.pi / 2
# Add to list
graspList.append(
Grasp('cylindrical',
Pose.makePoseFromTransform(graspMatrix)))
return graspList
def testConeGraspLocations():
cone = Cone(Pose(0, 0, 5, 0, 0, 0), 10, 5)
grasps = cone.planGrasps([1, 1, 1, 1])
print(grasps)
def planGrasps(self, graspParams, surfaceOffset = 1):
"""
Create each grasp assuming the origin of the shape is the global origin, and then multiply the grasp Pose by
the transformation matrix to put the grasp location in the global frame
:param graspParams: [array 1x4] Array for the 4 grasp parameters;
0. # of parallel planes
1. # of divisions of 360 degrees
2. # of grasp rotations
3. # of 180 degree rotations
:param surfaceOffset: [double] Distance to start the grasp away from the surface
:return: [array of Grasp Objects] List of grasp objects
"""
numberCubeSides = 6
parallelPlanes = int(graspParams[0]) # Has to at least one and must be odd
grasp180Rotations = int(graspParams[3]) # has to be a 1 or a 2
graspList = []
for i in range(numberCubeSides):
# Rotate the originPose to generate grasps for all the sides of the cube
# Change the lengths of the sides to properly line up with the new origin
if i == 0:
rotOriginMatrix = Pose.makeTranformfromPose(Pose(0, 0, 0, 0, 0, 0))
xAxisValue = self.length
yAxisValue = self.width
zAxisValue = self.height
if i == 1:
rotOriginMatrix = Pose.makeTranformfromPose(Pose(0, 0, 0, 0, 0, np.pi / 2))
xAxisValue = self.width
yAxisValue = self.length
zAxisValue = self.height
if i == 2:
rotOriginMatrix = Pose.makeTranformfromPose(Pose(0, 0, 0, 0, 0, np.pi))
xAxisValue = self.length
yAxisValue = self.width
zAxisValue = self.height
if i == 3:
rotOriginMatrix = Pose.makeTranformfromPose(Pose(0, 0, 0, 0, 0, -np.pi / 2))
xAxisValue = self.width
yAxisValue = self.length
zAxisValue = self.height
if i == 4:
rotOriginMatrix = Pose.makeTranformfromPose(Pose(0, 0, 0, np.pi / 2, 0, 0))
xAxisValue = self.width
yAxisValue = self.height
zAxisValue = self.length
if i == 5:
rotOriginMatrix = Pose.makeTranformfromPose(Pose(0, 0, 0, -np.pi / 2, 0, 0))
xAxisValue = self.width
yAxisValue = self.height
zAxisValue = self.length
originTransform = np.matmul(Pose.makeTranformfromPose(self.originPose), rotOriginMatrix)
# Generate parallel matrix representing the grasper's frame
translationMatrix = Pose.makeTranformfromPose(Pose(xAxisValue / 2, 0, -zAxisValue / 2, 0, 0, 0))
parallelMatrix = np.matmul(originTransform, translationMatrix)
for j in range(1, parallelPlanes + 1): # Vertical and Horizontal
for k in range(1, parallelPlanes + 1):
# The following translation matrices are used to move the frame
# horizontally and vertically along the side of the cube
transXMatrix = Pose.makeTranformfromPose(
Pose(-k * (xAxisValue / (parallelPlanes + 1)), 0, 0, 0, 0, 0))
transZMatrix = Pose.makeTranformfromPose(
Pose(0, 0, j * (zAxisValue / (parallelPlanes + 1)), 0, 0, 0))
# Compute the resulting matrix
divisionsMatrix = np.matmul(parallelMatrix, transXMatrix)
divisionsMatrix = np.matmul(divisionsMatrix, transZMatrix)
for l in range(grasp180Rotations * 2):
# The following two transforms move the frame out of the object and rotates
# the z axis into the object and the x axis perpendicular to the plane it will be grasping
transYMatrix = Pose.makeTranformfromPose(Pose(0, -(yAxisValue / 2) - surfaceOffset, 0, 0, 0, 0))
rotXMatrix = Pose.makeTranformfromPose(Pose(0, 0, 0, -np.pi / 2, 0, 0))
graspMatrix = np.matmul(divisionsMatrix, transYMatrix)
graspMatrix = np.matmul(graspMatrix, rotXMatrix)
# Rotate the thumb corresponding to the grasp180 rotation parameter
rotZMatrix = Pose.makeTranformfromPose(Pose(0, 0, 0, 0, 0, l * (-np.pi / 2)))
graspMatrix = np.matmul(graspMatrix, rotZMatrix)
# Create grasp and add it to the list of generated grasps
graspList.append(Grasp('cylindrical', Pose.makePoseFromTransform(graspMatrix)))
return graspList
def __init__(self, graspType='spherical', pose=Pose()):
self.graspType = graspType
self.pose = pose