Here is my code,
Code: Select all
import time
from coppeliasim_zmqremoteapi_client import RemoteAPIClient
import numpy as np
client=RemoteAPIClient()
sim=client.getObject('sim')
# simOMPL=client.getObject('simOMPL')
sim.startSimulation()
UR5Handle=sim.getObject('/UR5')
cuboid=sim.getObject('/Cuboid')
sim.setObjectSpecialProperty(UR5Handle,sim.objectspecialproperty_measurable)
sim.setObjectSpecialProperty(cuboid,sim.objectspecialproperty_measurable)
robotCollection = sim.createCollection(1)
sim.addItemToCollection(robotCollection,sim.handle_tree,UR5Handle,0)
joints=[]
joint1=sim.getObject('/joint1')
joints.append(joint1)
joint2=sim.getObject('/joint2')
joints.append(joint2)
joint3=sim.getObject('/joint3')
joints.append(joint3)
joint4=sim.getObject('/joint4')
joints.append(joint4)
joint5=sim.getObject('/joint5')
joints.append(joint5)
joint6=sim.getObject('/joint6')
joints.append(joint6)
print(joints)
# print(joint6,joint5,joint4,joint3,joint2,joint1)
# for i in range(6):
# joint1=sim.getObject('/joint'+str(i))
while(1):
result,collidingObjectHandle=sim.checkCollision(robotCollection,cuboid)
result2,distanceData,objectHandlePaire=sim.checkDistance(robotCollection,cuboid,0.1)
Joint=[]
for i,joint in enumerate(joints):
Joint.append(sim.getJointPosition(joint))
print(Joint)
time.sleep(0.5)
# print(result)
# print(Joint)
# print(distanceData)
[-1.149588486626385e-07, -1.4209752929872366e-08, 0.0, -1.659954973476374e-08, 7.664606727075807e-10, 8.881784197001252e-16]
Obviously, this return value is incorrect