Test your AI Agents with the all-new Agent to Agent Testing Platform.Learn More

How to use child_check method in pandera

Best Python code snippet using pandera_python

gym_test_joint.py

Source:gym_test_joint.py

1#!/usr/bin/env python2import random3import math4import gym5from gym import spaces6from gym.utils import seeding7import numpy as np8import time9import pybullet as p10import random11import pybullet_data12from modelInfo_util import ModelInfo13from pkg_resources import parse_version14import os, inspect15currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))16# print ("current_dir=" + currentdir)17os.sys.path.insert(0,currentdir)18from mamad_util import JointInfo19modelInfo = ModelInfo(path="./model_info.yml")20p.connect(p.GUI)21finger = p.loadSDF("./model.sdf")22fingerID = finger[0]23p.resetBasePositionAndOrientation(fingerID,[0.00000,0.200000,0.65000],[0.000000,0.000000,0.000000,1.000000])24jointInfo = JointInfo()25jointInfo.get_infoForAll_joints(finger)26active_joints_info  = jointInfo.getActiveJointsInfo()27num_active_joints = jointInfo.getNumberOfActiveJoints()28xpos = 0.0529ypos = 0.0430zpos = 131ang = 3.14*0.532orn = p.getQuaternionFromEuler([0,0,ang])33table = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"table/table.urdf"), 0.5000000,0.60000,0.0000,0.000000,0.000000,0.0,1.0)34texUid = p.loadTexture("./cube_new/aaa.png")35cube_objects = p.loadSDF("./cube_new/model.sdf")36cubeId = cube_objects[0]37p.changeVisualShape(cube_objects[0], -1,rgbaColor =[1,1,1,1])38p.changeVisualShape(cube_objects[0], -1, textureUniqueId = texUid)39p.resetBasePositionAndOrientation(cube_objects[0],(0.5000000,0.60000,0.6700),(0.717,0.0,0.0,0.717))40# print("active_joints_info::",active_joints_info)41# print("finger::",finger)42num_joints = p.getNumJoints(fingerID)43# print("`num of joints:::",num_joints)44blockUid = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"block.urdf"), xpos,ypos,zpos,orn[0],orn[1],orn[2],orn[3])45p.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf"),[0,0,0])46for i in range(num_joints-1):47	j_info = p.getJointInfo(fingerID,i)48	# print("joint_info::",j_info)49p.setTimeStep(1.0/1000.0)50p.setRealTimeSimulation(0)51link_name ="lbr_iiwa_link_7"52link_name_encoded = link_name.encode(encoding='UTF-8',errors='strict')53kuka_ee_link_jointInfo = jointInfo.searchBy(key="linkName",value =link_name_encoded )[0]54#print(kuka_ee_link_jointInfo)55kuka_ee_link_Index = 6#kuka_ee_link_jointInfo["jointIndex"]56# print("kuka_ee_link_Index",kuka_ee_link_Index)57blockPos,blockOrn = p.getBasePositionAndOrientation(blockUid)58#print(blockPos,blockOrn)59new_pos = [xpos,ypos,zpos]60new_orn = [orn[0],orn[1],orn[2],orn[3]]61#print("test1",new_pos)62#print("test2",new_orn)63jointPoses  = p.calculateInverseKinematics(fingerID,kuka_ee_link_Index,new_pos,orn)64joint_state =[]65hand_links = modelInfo.get_hand_links()66hand_links_info = []67for link_name in hand_links:68	link_info = modelInfo.searchBy(key="link",value=link_name)69	hand_links_info.append(link_info)70hand_links_info_with_active_joints = []71for i in hand_links_info:72	if i["joint"]["j_type"] != "fixed":73		hand_links_info_with_active_joints.append(i)74#print("test2::::::",hand_links_info_with_active_joints)75hand_indexOf_active_joints =[]76for Link in hand_links_info_with_active_joints:77    link = jointInfo.searchBy("linkName",Link["link_name"])[0]78    hand_indexOf_active_joints.append(link["jointIndex"])79#print("test3::::::",hand_indexOf_active_joints)80#print(hand_links_info_with_active_joints)81for i in hand_indexOf_active_joints:82	joint_pos = p.getJointState(fingerID,i)[0]83	# print("test2::",p.getJointState(fingerID,i))84	joint_state.append(joint_pos)85#new_jointPoses = jointPoses +joint_state86# print("test::",joint_state)87# print("jointPoses::",jointPoses)	88# print("len(jointPoses)::",len(jointPoses))89# print("num_active_joints::",num_active_joints)90jointPoses_joint_name = []91for i in range(num_active_joints):92	jointPoses_joint_name.append(active_joints_info[i]["jointName"])93"""94['J0', 'J1', 'J2', 'J3', 'J4', 'J5', 'J6',95 'lh_FFJ4', 'lh_FFJ3', 'lh_FFJ2', 'lh_FFJ1',96 'lh_MFJ4', 'lh_MFJ3', 'lh_MFJ2', 'lh_MFJ1',97 'lh_RFJ4', 'lh_RFJ3', 'lh_RFJ2', 'lh_RFJ1', 98 'lh_THJ5', 'lh_THJ4', 'lh_THJ2', 'lh_THJ1']99lh_FFJ3 ll:0.0  ul:1.5708100lh_MFJ3 ll:0.0  ul:1.5708101lh_RFJ3 ll:0.0  ul:1.5708102lh_FFJ4 ll:-0.349066  ul:0.349066103lh_MFJ4 ll:-0.349066  ul:0.349066104lh_RFJ4 ll:-0.349066  ul:0.349066105"""106jointPoses = (2.0, 1.0232196872785049, 0.15139632616115659, 2.640651350178653, -1.4544423913682578, 2.8769639265642334, 0.0,107 			 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)108jointPoses = list(jointPoses)		  109#cuzing collsion between fingers			  110# changing lh_FFJ3111jointPoses[8] = jointPoses[8]112# changing lh_MFJ4 index = 13113jointPoses[11] = jointPoses[12]+0.3114# changing lh_RFJ3 index=17115jointPoses[16] = jointPoses[16]116contact_set = []117child_check = []118while(1):119	for i in range(num_active_joints):120		jointIndex = active_joints_info[i]["jointIndex"]121		jointll = active_joints_info[i]["jointLowerLimit"]122		jointul = active_joints_info[i]["jointUpperLimit"]123		jointState = p.getJointState(fingerID,jointIndex)[0]124		#print("jointName::",active_joints_info[i]["jointName"])125		#print("jointIndex::",jointIndex)126		# print("jointll::",jointll)127		# print("jointul::",jointul)128		#print("jointState::",jointState)129		#print(joint)130		# print("test:::",jointState)131		motor_command = (0.5,0.5,0.5)132		#print(motor_command)133	 	p.setJointMotorControl2(fingerID,7,p.POSITION_CONTROL,motor_command, targetVelocity=0,force=200.0, maxVelocity=0.35,positionGain=0.3,velocityGain=1)134	135	136	index_of_actvie_joints = [active_joints_info[i]["jointIndex"] for i in range(num_active_joints)]137	#print("index_of_actvie_joints",index_of_actvie_joints)138	for index_1 in index_of_actvie_joints:139		for index_2 in index_of_actvie_joints:140			if index_1 == index_2:141				continue142				143			contact=p.getClosestPoints(fingerID,fingerID,0.0,index_1,index_2)144			#print(contact)145			# contact=p.getClosestPoints(fingerID,fingerID,0.0)146			if len(contact) !=0:147				# print("contact",contact)148				# print("\n")149				# print("contact[0][3]",contact[0][3])150				link_one_name = jointInfo.searchBy("jointIndex",contact[0][3])[0]["linkName"]151				link_two_name = jointInfo.searchBy("jointIndex",contact[0][4])[0]["linkName"]152				#print("link_one_name dododod",jointInfo.searchBy("jointIndex",contact[0][3])[0])153				# print("link_one_name",link_one_name)154				contact_set.append([contact[0][3],contact[0][4]])155				#child_check.append([contact[0][3],contact[0][4]])156	#print("contact_set::",contact_set)157	#new_contact = []158	#new_contact = list(set(contact_set))159	#print(new_contact)160	#print("::",len(contact_set))161	new_contact = []162	for i in contact_set:163		#set1 = contact_set[i][0]164		#set2 = contact_set[i][1]165		#for i in len(contact_set):166			#if (not contact[i][0] in set1) and (not contact[i][1] in set2):167				#new_contact = 168		if not i in new_contact:169			new_contact.append(i)170	#print("new_contact::",new_contact)171	parent_set = []172	#print(len(new_contact))173	for i in range(len(new_contact)):174		#print(new_contact[i][0])175		if new_contact[i][0] - new_contact[i][1] == 1:176			parent_set.append(new_contact[i])177	#print(parent_set)178	child_set = []179	for i in range(len(new_contact)):180		if new_contact[i][1] - new_contact[i][0] == 1:181			child_set.append(new_contact[i])182	#print(child_set)183	parent_check = []184	for i in new_contact:185		for j in parent_set:186			if i == j:187				break188		else:189			parent_check.append(i)190	#print(parent_check)191	child_check = []192	for i in parent_check:193		for j in child_set:194			if i == j:195				break196		else:197			child_check.append(i)198	check_flip=[]199	for i in range(len(child_check)):200		index_1=child_check[i][0]201		index_2=child_check[i][1]202		for j in range(i,len(child_check)):203			if i == j:204				continue205			if index_1 == child_check[j][1] and index_2 ==  child_check[j][0]:206				check_flip.append(j)207	new_check=[]208	sort=np.argsort(check_flip)209	for i in range(len(check_flip)):210		new_check.append(check_flip[sort[i]])211	for i in range(len(check_flip)):212		del child_check[new_check[i]-i]213	#print(child_check)214	collision_result = []215	for i in range (len(child_check)):216		index_collision_set_1=child_check[i][0]217		index_collision_set_2=child_check[i][1]218		for j in range(num_active_joints):219			if index_collision_set_1 == active_joints_info[j]["jointIndex"]:220				index_collision_set_1_result = j221			if index_collision_set_2 == active_joints_info[j]["jointIndex"]:222				index_collision_set_2_result = j	223		collision_result.append([active_joints_info[index_collision_set_1_result]["linkName"],\224		active_joints_info[index_collision_set_2_result]["linkName"]])225	print("right hand self coliision -------",child_check)226	print("right hand self coliision -------",collision_result)227	print("\n")228p.stepSimulation()229	# print("jointPoses_joint_name",jointPoses_joint_name)230	# print("joint values form ik",jointPoses)231	# print("len(joint values form ik)",len(jointPoses))232	# print("getPhysicsEngineParameters",p.getPhysicsEngineParameters())233'''234	#print("contact_set::",contact_set)235	#new_contact = []236	#new_contact = list(set(contact_set))237	#print(new_contact)238	#print("::",len(contact_set))239	new_contact = []240	for i in contact_set:241		#set1 = contact_set[i][0]242		#set2 = contact_set[i][1]243		#for i in len(contact_set):244			#if (not contact[i][0] in set1) and (not contact[i][1] in set2):245				#new_contact = 246		if not i in new_contact:247			new_contact.append(i)248	#print("new_contact::",new_contact)249	parent_set = []250	#print(len(new_contact))251	for i in range(len(new_contact)):252		#print(new_contact[i][0])253		if new_contact[i][0] - new_contact[i][1] == 1:254			parent_set.append(new_contact[i])255	#print(parent_set)256	child_set = []257	for i in range(len(new_contact)):258		if new_contact[i][1] - new_contact[i][0] == 1:259			child_set.append(new_contact[i])260	#print(child_set)...

Automation Testing Tutorials

Learn to execute automation testing from scratch with LambdaTest Learning Hub. Right from setting up the prerequisites to run your first automation test, to following best practices and diving deeper into advanced test scenarios. LambdaTest Learning Hubs compile a list of step-by-step guides to help you be proficient with different test automation frameworks i.e. Selenium, Cypress, TestNG etc.