def get_joint_data(clientID):
joints = {}
_, ids, _, _, names = sim.simxGetObjectGroupData(
clientID, simConst.sim_object_joint_type, 0, opmode)
for i in range(len(names)):
joints[names[i]] = sim.simxGetObjectOrientation(
clientID, ids[i], -1, opmode)[1]
return joints
def _get_info_about_joint(self, handle):
obj_type_code = sc.sim_object_joint_type
data_type_code = 16
code, handles, types_and_mode, limits_and_ranges, string_data = s.simxGetObjectGroupData(
self._id, obj_type_code, data_type_code, self._def_op_mode)
if code == sc.simx_return_ok:
index = handles.index(handle)
index = index * 2
return types_and_mode[index], types_and_mode[index+1], limits_and_ranges[index], limits_and_ranges[index+1]
else:
return None
def get_joint_handle(clientID, object_type=simConst.sim_object_joint_type, mode=opmode):
UR3_joints={}
_, ids,_,_,names= sim.simxGetObjectGroupData(clientID,object_type, 0,mode)
#_, ids,_,rot,_= sim.simxGetObjectGroupData(clientID,object_type, 5,mode)
c=0
#d=0
for i in names:
UR3_joints[i]=[ids[c]]
#UR3_joints[i]=[ids[c], rot[d:d+3]]
c=c+1
#d=d+3
return UR3_joints
def _check_object_type(self, handle, obj_type):
code, handles, _, _, _ = s.simxGetObjectGroupData(
self._id, obj_type, 0, self._def_op_mode)
return handles.__contains__(handle)
import numpy as np
import math
import time
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000,
5) # Connect to V-REP
params = pkl.load(open('learnt_params.pkl', 'rb'))
if clientID == -1:
sys.exit("Couldn't connect to CSim server")
else:
print("Connected to Sim Server")
output = sim.simxGetObjectGroupData(clientID, sim.sim_appobj_object_type, 0,
sim.simx_opmode_blocking)
object_handles_list = output[1]
object_names_list = output[-1]
object_handles_dict = dict(zip(object_names_list, object_handles_list))
snakes_v_joints = {k: []
for k in ['0', '8', '17', '26', '35', '44']
} #,'53','62','71','80','89']}
snakes_h_joints = {k: []
for k in ['0', '8', '17', '26', '35', '44']
} #,'53','62','71','80','89']}
for i in range(len(object_names_list)):
if 'vJoint' in str(object_names_list[i]) or 'hJoint' in str(
object_names_list[i]):
try: