def update_debug_parameters():
global last_cube_update
cube = None
last_pos, last_angle, last_size = (None, None, None)
while p.isConnected():
rgb = [p.readUserDebugParameter(id) for id in rgb_ids] + [1]
_theta, _rho, _angle = (p.readUserDebugParameter(theta) / 180.0 *
np.pi, p.readUserDebugParameter(rho),
p.readUserDebugParameter(angle) / 180.0 *
np.pi)
_cube_size = p.readUserDebugParameter(cube_size) * 0.125
pos = (np.cos(_theta) * _rho, np.sin(_theta) * _rho,
_cube_size / 2.0)
if last_pos != pos or last_angle != _angle or last_size != _cube_size:
if cube is not None:
last_cube_removal = time.time()
cube.remove()
cube = Cube(pos, (_angle, 0, 0), rgb, _cube_size)
last_pos = pos
last_angle = _angle
last_size = _cube_size
continue
elif cube is not None:
p.changeVisualShape(cube.body, -1, rgbaColor=rgb)
time.sleep(0.2)
def _disconnect_from_pybullet(self):
"""Disconnect from the simulation.
Disconnects from the simulation and sets simulation to disabled to
avoid any further function calls to it.
"""
if pybullet.isConnected(physicsClientId=self._pybullet_client_id):
pybullet.disconnect(physicsClientId=self._pybullet_client_id, )
def _disconnect_from_pybullet(self):
"""Disconnect from the simulation.
Disconnects from the simulation and sets simulation to disabled to
avoid any further function calls to it.
"""
if pybullet.isConnected():
pybullet.disconnect()
def __del__(self):
"""
Removes the visual object from the environment
"""
# At this point it may be that pybullet was already shut down. To avoid
# an error, only remove the object if the simulation is still running.
if p.isConnected():
p.removeBody(self.body_id)
def run_sim(self):
"""starts the main loop of the simulation"""
while pybullet.isConnected(self.physicsClientId):
if not self.sim.paused:
self.control_system.step() # updates the bots' control-system
self.timer.step_simulation() # step physics
print("\n--simulation terminated--")
def __del__(self):
"""
Removes the block from the environment
"""
# At this point it may be that pybullet was already shut down. To avoid
# an error, only remove the object if the simulation is still running.
if pybullet.isConnected(**self._kwargs):
pybullet.removeBody(self.block, **self._kwargs)
def main():
physicsClient = p.connect(p.GUI)
p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
p.setGravity(0, 0, -10)
#data path to search object meshes
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
clothId = p.loadSoftBody(fileName=os.path.join(assets_path, 'tshirt.obj'),
basePosition=[0, 0, 1.35],
baseOrientation=[0, 0, 0.7071068, 0.7071068],
scale=.9,
collisionMargin=0.02,
useMassSpring=1,
mass=1,
springElasticStiffness=10,
springDampingStiffness=0.2,
useBendingSprings=1,
useFaceContact=1)
# clothId = p.loadSoftBody(fileName=os.path.join(assets_path, 'tshirt_mia.obj'), basePosition=[0.02, 0, -0.05], baseOrientation=[ 0.5, 0.5, 0.5, 0.5 ], scale=.35,
# collisionMargin=0.05, useMassSpring=1, mass=1, springElasticStiffness=8, springDampingStiffness=0.2, useBendingSprings=1)
# cloth_mesh = p.getMeshData(clothId)
# print(cloth_mesh[0], len(cloth_mesh[1]))
# clothId = p.loadSoftBody(fileName=os.path.join(assets_path, 'hospitalgown_adaptivereduce.obj'), basePosition=[0, 0, 2.35], baseOrientation=[ 0, 0, 0.7071068, 0.7071068 ], scale=.9,
# collisionMargin=0.02, useMassSpring=1, mass=1, springElasticStiffness=10, springDampingStiffness=0.2, useBendingSprings=1, useFaceContact=1)
humanoid = MyHumanoid(p)
humanoid.fixBase()
runSimulation = False
useRealTimeSimulation = 0
cubeId = p.loadURDF("cube_small.urdf", [0.1, 0.05, 1.6], [1, 0, 0, 0],
True, True)
#use official anchor feature for deformable body
# p.createSoftBodyAnchor(clothId ,3,cubeId,-1, [0.5,-0.5,0])
# p.setRealTimeSimulation(1)
while p.isConnected():
keys = p.getKeyboardEvents()
if ord('q') in keys and keys[ord('q')] & p.KEY_WAS_TRIGGERED:
break
if ord(' ') in keys and keys[ord(' ')] & p.KEY_WAS_TRIGGERED:
runSimulation = not runSimulation
if runSimulation:
p.stepSimulation()
# p.setGravity(0,0,-10)
# sleep(1./240.)
return
def run(self):
"""
This method initializes the new simulation and checks in an endless loop if it is still active. If it is the
thread will be suspended for 10 seconds, if it is not the method and thus the thread terminates.
"""
if self.type == "GUI":
self.world.client_id = p.connect(p.GUI)
else:
self.world.client_id = p.connect(p.DIRECT)
while p.isConnected(self.world.client_id):
time.sleep(10)
def _connect(self):
assert p.isConnected(), 'PyBullet not connected.'
self._axis_ids = []
keys = list(self._action.keys())
for i in range(6):
self._axis_ids.append(
p.addUserDebugParameter(
paramName=keys[i],
rangeMin=-1,
rangeMax=1,
startValue=0))
self._is_connected = True
def simulator(Initial_pos=[0, 0, 0], Initial_angle=[0, 0, 0]):
"""
This function is used to do simulation for the gripper.
:param Initial_pos: The initial position of the barrett hand.
It should be a list with three values which means at x, y, z directions.
:param Initial_angle: The initial orientation of the barrett hand.
It should be a list with three values which means the rotation angle at x, y, z directions.
The unit of the rotation angle is the radian system.
:return: The state of the gripper.
"""
# connect physical server
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
# set gravity
p.setGravity(0, 0, 0)
# create an empty link list to store link id
LinkId = []
# model initial location
StartPos = Initial_pos
# model initial orientation in Euler
StartOrientation = p.getQuaternionFromEuler(Initial_angle)
# load model file and set the initial position and fixed base link
boxId = p.loadURDF("barrett_hand_target.urdf", StartPos, StartOrientation, useFixedBase=True)
# load object model
object = p.loadURDF("object.urdf", useFixedBase=True)
# set gripper be the loaded model
gripper = boxId
# set camera parameters
p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=45, cameraPitch=0, cameraTargetPosition=[0.5, 0.5, 0.2])
# for loop to obtain the joint information and set parameters
for i in range(0, p.getNumJoints(gripper)):
p.setJointMotorControl2(gripper, i, p.POSITION_CONTROL, targetPosition=0, force=0)
# obtain the limit rotation angle range of the joint
lower_limit = p.getJointInfo(gripper, i)[8]
upper_limit = p.getJointInfo(gripper, i)[9]
# obtain the joint name
linkName = p.getJointInfo(gripper, i)[12].decode("ascii")
# set the gui control board
LinkId.append(p.addUserDebugParameter(linkName, lower_limit, upper_limit, 0))
while p.isConnected():
# start do simulation
p.stepSimulation()
time.sleep(1. / 240.)
# move joints following command
for i in range(0, p.getNumJoints(gripper)):
linkPos = p.readUserDebugParameter((LinkId[i]))
p.setJointMotorControl2(gripper, i, p.POSITION_CONTROL, targetPosition=linkPos)
p.saveBullet('Test.bullet')
p.disconnect()
def run(self):
# Prepare World Description -> List formatting
includeBase = False
listElems = utils_collision.buildListElems(p) #Renvoie une liste [body,link] pour le world
logging.info("Liste des bodyLink" + str(listElems))
self.listCouples = utils_collision.buildListCoupleLinks(listElems, includeBase)
logging.info("Liste des paires collisions" + str(self.listCouples))
"""Conserve juste les paires pour lesquelles le calcul de collision est nécessaire (pas de self-coll, pas de base, pas d'importance d'ordre)
"""
# Declare positions and assign to default values"
while p.isConnected():
#MANUAL_CONTROL est un état qui permet le guidage via les boutons ou via des consignes
"""self.positionJoint0 = p.readUserDebugParameter(self.positionJoint0Id) * 6.28 / 360 #Read the input parameters from sliders
self.positionJoint1 = p.readUserDebugParameter(self.positionJoint1Id) * 6.28 / 360
self.positionJoint2 = p.readUserDebugParameter(self.positionJoint2Id) * 6.28 / 360
p.setJointMotorControl2(self.world.ppsId, 1, p.POSITION_CONTROL, targetPosition=self.positionJoint0)
p.setJointMotorControl2(self.world.ppsId, 2, p.POSITION_CONTROL, targetPosition=self.positionJoint1)
p.setJointMotorControl2(self.world.ppsId, 3, p.POSITION_CONTROL, targetPosition=self.positionJoint2)"""
#logging.info("Voici la valeur de positionJoint0: " + str(self.positionJoint0))
#logging.info("Voici la valeur de positionJoint1: " + str(self.positionJoint1))
#logging.info("Voici la valeur de positionJoint2: " + str(self.positionJoint2))
# sleep is NOT required, but no need to compute more than that.
time.sleep(0.05)
'''
Get the closest points between two objects
'''
self.results = utils_collision.compute(p, self.listCouples, distanceMin=self.threshold, listOfExclusion=None) #J'ai besoin de chopper le results
#logging.info("Minimal Distance Computed => " + str(min(self.results[2])))
ind = self.results[2].index(min(self.results[2]))
showFrom = self.results[1][ind][0]
showTo = self.results[1][ind][1]
for i in range(0,3):
CoM = p.getLinkState(0, i+1)[0]
otherPoint = [CoM[0],CoM[1],CoM[2]+100]
p.addUserDebugLine(lineFromXYZ=CoM, lineToXYZ=otherPoint, lineColorRGB=[0, 0, 1], lineWidth=4,
lifeTime=0.5)
p.addUserDebugLine(lineFromXYZ=showFrom, lineToXYZ=showTo, lineColorRGB=[1, 1, 0], lineWidth=2,
lifeTime=0.5)
return
def start_device_manager(self, client_id, bot_id):
self.client_id = client_id
self.bot_id = bot_id
assert pb.isConnected(self.client_id) == True
while self.enc is None:
self._read_encoder()
while self.gyro is None:
self._read_gyro()
while self.tilt is None:
self._read_tilt()
self.thread_read_on = True
self.thread_read = threading.Thread(target=self._read)
self.thread_read.start()
def pepper_track_object():
found_left, found_right, found_mid = False, False, False
while p.isConnected():
if time.time() - last_cube_update < 2:
found_left, found_right, found_mid = False, False, False
time.sleep(0.5)
right, mid, left = (engine._agent.getRightLaserValue(),
engine._agent.getFrontLaserValue(),
engine._agent.getLeftLaserValue())
found_mid = False
for j in mid:
if j < 2.8:
found_mid = True
found_left = False
found_right = False
break
if not found_mid:
for j in right:
if j < 2.8:
found_right = True
found_left = False
break
for j in left:
if j < 2.8:
found_left = True
found_right = False
break
if not found_right:
if found_left:
engine._agent.move(0, 0, np.pi / 2.0)
else:
if random.random() > 0.5:
found_left = True
else:
found_right = True
else:
engine._agent.move(0, 0, -np.pi / 2.0)
else:
tmp = []
for j in range(len(mid)):
if (mid[j] != 3.0):
tmp += [j]
center = np.mean(tmp)
dif = (center - len(mid) / 2.0) / len(mid) * 2.0
engine._agent.move(0, 0, -np.pi / 2.0 * dif)
def test_robot():
c = p.connect(p.SHARED_MEMORY)
if (c < 0):
c = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
p.loadURDF("plane.urdf") #loads from the root pybullet library
p.setGravity(0, 0, -10.0)
p.setRealTimeSimulation(0)
#set up the robot and the ball
max_joint_torque = 100
omnid_simulator = Omnid_Simulator()
timeStep = 0.0005
p.setTimeStep(timeStep)
total_step = 0
initialized = False
omnid_simulator.attachBallToRobot(
) # we want the robot to land safely onto the robot.
#setup user debug tools if needed
debug_tools = modelTestSetup(max_joint_torque)
while p.isConnected():
observation = omnid_simulator.updateStates()
#TODO
if not initialized:
if omnid_simulator.ballonRobot():
omnid_simulator.detachBallFromRobot(
) #now we can let the ball move freely!
initialized = True
print("initialized")
torqueDict = {
"theta_1": p.readUserDebugParameter(debug_tools[0]),
"theta_2": p.readUserDebugParameter(debug_tools[1]),
"theta_3": p.readUserDebugParameter(debug_tools[2])
}
omnid_simulator.applyJointTorque(torqueDict)
total_step += 1
p.stepSimulation()
def step(self):
if not p.isConnected(self.client_id):
return
# get keyboard events if gui is active
single_step = False
if self.gui:
# reset if R-key was pressed
r_key = ord('r')
n_key = ord('n')
s_key = ord('s')
f_key = ord('f')
space_key = p.B3G_SPACE
keys = p.getKeyboardEvents()
if r_key in keys and keys[r_key] & p.KEY_WAS_TRIGGERED:
self.reset()
if space_key in keys and keys[space_key] & p.KEY_WAS_TRIGGERED:
self.paused = not self.paused
if s_key in keys and keys[s_key] & p.KEY_IS_DOWN:
single_step = True
if n_key in keys and keys[n_key] & p.KEY_WAS_TRIGGERED:
if self.gravity:
p.setGravity(0, 0, 0)
else:
p.setGravity(0, 0, -9.81)
self.gravity = not self.gravity
if f_key in keys and keys[f_key] and p.KEY_WAS_TRIGGERED:
self.follow_camera = not self.follow_camera
backspace_key = p.B3G_BACKSPACE
if backspace_key in keys and keys[backspace_key] & p.KEY_WAS_TRIGGERED:
p.disconnect(self.client_id)
return
# check if simulation should continue currently
if not self.paused or single_step:
self.time += self.time_step
p.stepSimulation()
if self.follow_camera:
camera = p.getDebugVisualizerCamera()
p.resetDebugVisualizerCamera(cameraDistance=camera[10], cameraYaw=camera[8], cameraPitch=camera[9],
cameraTargetPosition=p.getBasePositionAndOrientation(self.robot_index)[0])
def init_sim():
if not p.isConnected():
globals.physicsClient = p.connect(
p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
else:
p.resetSimulation(globals.physicsClient)
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
globals.sph1 = p.loadURDF("robots/capsule01.urdf", [0, 0, 3],
cubeStartOrientation)
globals.sph2 = p.loadURDF("robots/sphere01_man.urdf", [0, 0, 5],
cubeStartOrientation)
globals.t_step = p.getPhysicsEngineParameters()['fixedTimeStep']
globals.curr_step = 0
p.setJointMotorControl2(globals.sph1,
0,
controlMode=p.VELOCITY_CONTROL,
force=0)
def main():
# initialize robot
robot = skrobot.models.Kuka()
interface = skrobot.interfaces.PybulletRobotInterface(robot)
pybullet.resetDebugVisualizerCamera(
cameraDistance=1.5,
cameraYaw=45,
cameraPitch=-45,
cameraTargetPosition=(0, 0, 0.5),
)
print('==> Initialized Kuka Robot on PyBullet')
for _ in range(100):
pybullet.stepSimulation()
time.sleep(3)
# reset pose
print('==> Moving to Reset Pose')
robot.reset_manip_pose()
interface.angle_vector(robot.angle_vector(), realtime_simulation=True)
interface.wait_interpolation()
time.sleep(3)
# ik
print('==> Solving Inverse Kinematics')
target_coords = skrobot.coordinates.Coordinates(pos=[0.5, 0, 0]).rotate(
np.pi / 2.0, 'y', 'local')
skrobot.interfaces.pybullet.draw(target_coords)
robot.inverse_kinematics(
target_coords,
link_list=robot.rarm.link_list,
move_target=robot.rarm_end_coords,
rotation_axis=True,
stop=1000,
)
interface.angle_vector(robot.angle_vector(), realtime_simulation=True)
interface.wait_interpolation()
# wait
while pybullet.isConnected():
time.sleep(0.01)
def run(self):
# Le but de ce thread va être de calculer la séquence de position que doit effectuer nos objets
time.sleep(
1
) # On attends une petite seconde le temps que l'autre thread commence à run
#self.get_final_state()
row = 0
#self.construct_C_space()
while p.isConnected():
#self.construct_C_space()
self.update_arrays_att()
#self.print_array_att_leoni()
self.update_arrays_rep()
self.get_att_fields()
self.get_rep_fields()
self.write_joint_pos(row)
#self.print_array_rep_leoni()
self.update_jacobians()
#self.print_array_jac_att()
#self.print_array_jac_rep()
self.get_world_rep_forces()
self.get_world_att_forces()
#self.print_rep_forces()
self.print_att_forces()
self.proj_world_forces()
self.step_forward()
#self.print_joint_torques()
#self.print_joint_torques(jointTorques)
#self.print_joint_pos_deg()
self.reinit_arrays_rep() #Réinitialisation de la distance
self.clear_arrays_forces()
row += 1
#self.print_position_all_link(self.world.ppsId)
print("finito")
return
def __init__(self, time_step: SimulationTime, gui: bool = False):
self._gui = gui
if self._INSTANCE is not None and p.isConnected(self._INSTANCE._physics_client_id):
# Instance could already exist when running test, just to be sure resetting it here.
p.resetSimulation(self._INSTANCE._physics_client_id)
self._physics_client_id = -1
if gui:
self._physics_client_id = p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_KEYBOARD_SHORTCUTS, 0)
p.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
else:
self._physics_client_id = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setTimeStep(time_step.seconds)
p.setGravity(0, 0, -GRAVITY)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.setGravity(0,0,-10)
p.loadURDF("plane.urdf",[0,0,-1])
p.loadURDF("r2d2.urdf")
pixelWidth = 320
pixelHeight = 220
camTargetPos = [0,0,0]
camDistance = 4
pitch = -10.0
roll=0
upAxisIndex = 2
while (p.isConnected()):
for yaw in range (0,360,10) :
start = time.time()
p.stepSimulation()
stop = time.time()
print ("stepSimulation %f" % (stop - start))
#viewMatrix = [1.0, 0.0, -0.0, 0.0, -0.0, 0.1736481785774231, -0.9848078489303589, 0.0, 0.0, 0.9848078489303589, 0.1736481785774231, 0.0, -0.0, -5.960464477539063e-08, -4.0, 1.0]
viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex)
projectionMatrix = [1.0825318098068237, 0.0, 0.0, 0.0, 0.0, 1.732050895690918, 0.0, 0.0, 0.0, 0.0, -1.0002000331878662, -1.0, 0.0, 0.0, -0.020002000033855438, 0.0]
start = time.time()
img_arr = p.getCameraImage(pixelWidth, pixelHeight, viewMatrix=viewMatrix, projectionMatrix=projectionMatrix, shadow=1,lightDirection=[1,1,1])
stop = time.time()
print ("renderImage %f" % (stop - start))
#time.sleep(.1)
def main():
p.setRealTimeSimulation(1)
p.setGravity(0, 0, 0) # we don't use gravity yet
# debug colors only for show
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1], [0, 0, 0, 1] ]
currentColor = 0
try:
body = p.getBodyUniqueId(mantis_robot)
EndEffectorIndex = p.getNumJoints(body) -1
print("EndEffectorIndex %i" % EndEffectorIndex)
tx = 0
ty = 5
tz = 3
tj = 0
tp = 0
tr = math.pi / 2
orn = p.getQuaternionFromEuler([tj,tp,tr])
jointT = p.calculateInverseKinematics(body, EndEffectorIndex, [tx, ty,tz], orn, jointDamping=jd)
p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT )
print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr))
print(jointT)
p.resetBasePositionAndOrientation(target, [tx, ty, tz ], orn)
loop = 0
hasPrevPose = 0
trailDuration = 15
targetSelected = False
distance = 5
yaw = 90
motorDir = [1,-1,1,1,1,1]
while (p.isConnected()):
time.sleep(1. / 240.) # set to 240 fps
p.stepSimulation()
# get target positon
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
# do Inverse Kinematics
# jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, targetOrn)
jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, orn)
# set Robot Joints
p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT )
# reduce the output speed to 1/10 of the simulation speed
# ~24 fps
loop +=1
if loop > 10:
loop = 0
# sends data to the robot over ethernet/UDP
# just a simple string with the motor number and the angle positon in 12 bit range (0-4096)
data_string = ""
for i in range ( 1, EndEffectorIndex):
jt = p.getJointState(body, i )
data_string += "motor%i:%i\r\n" % ( i, scale(motorDir[i-1] * jt[0]))
transfer_socket.sendto(bytes(data_string, "utf-8"), (transfer_ip, transfer_port))
# generates the trail for debug only
# optional
ls = p.getLinkState(body, EndEffectorIndex)
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
if (hasPrevPose):
p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = targetPos
prevPose1 = ls[4]
hasPrevPose = 1
# get mouse events
# changes color of the object clickt on and prints some info
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_RELEASED) and (targetSelected == True) ):
# after mouse lost reset posion to prevent the target from floting around
targetSelected = False
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
p.resetBasePositionAndOrientation(target, targetPos, targetOrn)
if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
jointUid = hit[1]
if( objectUid == target):
targetSelected = True
if (objectUid >= 1):
# this is for debug click on an object to get id
# oject will change color this has no effect
print("obj %i joint %i" % (objectUid , jointUid))
p.changeVisualShape(objectUid, jointUid, rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0
except KeyboardInterrupt:
print("KeyboardInterrupt has been caught.")
finally:
endProgramm()
import pybullet as p
import time
useDirect = False
usePort = True
p.connect(p.GUI)
id = p.loadPlugin("grpcPlugin")
#dynamically loading the plugin
#id = p.loadPlugin("E:/develop/bullet3/bin/pybullet_grpcPlugin_vs2010_x64_debug.dll", postFix="_grpcPlugin")
#start the GRPC server at hostname, port
if (id < 0):
print("Cannot load grpcPlugin")
exit(0)
if usePort:
p.executePluginCommand(id, "localhost:12345")
else:
p.executePluginCommand(id, "localhost")
while p.isConnected():
if (useDirect):
#Only in DIRECT mode, since there is no 'ping' you need to manually call to handle RCPs:
numRPC = 10
p.executePluginCommand(id, intArgs=[numRPC])
else:
dt = 1. / 240.
time.sleep(dt)
def __del__(self):
"""Removes the cuboid from the environment."""
# At this point it may be that pybullet was already shut down. To avoid
# an error, only remove the object if the simulation is still running.
if pybullet.isConnected(self._pybullet_client_id):
pybullet.removeBody(self.body_id, self._pybullet_client_id)
def run(self):
while p.isConnected():
self.set_camera()
time.sleep(0.4)
return
def main():
p.setRealTimeSimulation(1)
p.setGravity(0, 0, 0) # we don't use gravity yet
# debug colors only for show
colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1],
[0, 0, 0, 1]]
currentColor = 0
try:
print("open SpaceMouse")
print(spacenavigator.list_devices())
success = spacenavigator.open()
body = p.getBodyUniqueId(mantis_robot)
EndEffectorIndex = p.getNumJoints(body) - 1
print("")
print("EndEffectorIndex %i" % EndEffectorIndex)
# i = 0
# for i in range(0, p.getNumJoints(body)):
# n = p.getJointInfo(body,i)
# alpha robot orientierung
# orn = p.getQuaternionFromEuler([math.pi/2,0,math.pi/2])
# p.resetBasePositionAndOrientation(body, [0, 0, 2 ], orn)
tx = 0
ty = 5
tz = 3
tj = 0
tp = 0
tr = math.pi / 2
orn = p.getQuaternionFromEuler([tj, tp, tr])
jointT = p.calculateInverseKinematics(body,
EndEffectorIndex, [tx, ty, tz],
orn,
jointDamping=jd)
p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6],
controlMode=p.POSITION_CONTROL,
targetPositions=jointT)
print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx, ty, tz, tj, tp, tr))
print(jointT)
p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn)
loop = 0
hasPrevPose = 0
trailDuration = 15
distance = 5
yaw = 90
motorDir = [1, -1, 1, 1, 1, 1]
while (p.isConnected()):
time.sleep(1. / 240.) # set to 40 fps
p.stepSimulation()
# targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
if success:
state = spacenavigator.read()
# print(state)
tx += state[1] * 0.01
ty += state[2] * 0.01
tz += state[3] * 0.01
# tj += state[4]*0.005
tp += state[5] * 0.005
tr += state[6] * -0.005
# print("x:%f y:%f z:%f j:%f p:%f r:%f " % (state[1],state[2],state[3],state[4],state[5],state[6]))
# orn = p.getQuaternionFromEuler([tj,tp,tr])
orn = p.getQuaternionFromEuler([tj, tp, tr])
jointT = p.calculateInverseKinematics(body, EndEffectorIndex,
[tx, ty, tz], orn)
p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6],
controlMode=p.POSITION_CONTROL,
targetPositions=jointT)
# print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr))
p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn)
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
# p.resetDebugVisualizerCamera(distance, yaw, 20, targetPos)
loop += 1
if loop > 10:
loop = 0
data_string = ""
for i in range(1, EndEffectorIndex):
jt = p.getJointState(body, i)
data_string += "motor%i:%i\r\n" % (
i, scale(motorDir[i - 1] * jt[0]))
transfer_socket.sendto(bytes(data_string, "utf-8"),
(transfer_ip, transfer_port))
ls = p.getLinkState(body, EndEffectorIndex)
targetPos, targetOrn = p.getBasePositionAndOrientation(target)
if (hasPrevPose):
p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1,
trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1,
trailDuration)
prevPose = targetPos
prevPose1 = ls[4]
hasPrevPose = 1
# get mouse events
mouseEvents = p.getMouseEvents()
for e in mouseEvents:
if ((e[0] == 2) and (e[3] == 0)
and (e[4] & p.KEY_WAS_TRIGGERED)):
mouseX = e[1]
mouseY = e[2]
rayFrom, rayTo = getRayFromTo(mouseX, mouseY)
rayInfo = p.rayTest(rayFrom, rayTo)
for l in range(len(rayInfo)):
hit = rayInfo[l]
objectUid = hit[0]
jointUid = hit[1]
if (objectUid >= 1):
# this is for debug click on an object to get id
# oject will change color this has no effect
# changing color real time seems to slow
print("obj %i joint %i" % (objectUid, jointUid))
# n = p.getJointInfo(objectUid,jointUid)
n = p.getJointState(objectUid, jointUid)
print(n)
p.changeVisualShape(objectUid,
jointUid,
rgbaColor=colors[currentColor])
currentColor += 1
if (currentColor >= len(colors)):
currentColor = 0
except KeyboardInterrupt:
print("KeyboardInterrupt has been caught.")
finally:
endProgramm()
in_path = path
path = root + '_vhacd' + ext
if not os.path.exists(path):
self._client.vhacd(in_path, path, '')
cid = self._client.createCollisionShape(pb.GEOM_MESH, fileName=path)
return self._client.createMultiBody(0.2, cid, vid)
raise RuntimeError(f'Cannot load model from: "{path}"')
def _get_observation(self):
"""Compute observation of the current environment.
Returns:
observation (object): agent's observation of the current environment
"""
if self._body_id >= 0:
hands_observation = super()._get_observation()
base_pos, base_orn = self._client.getBasePositionAndOrientation(self._body_id)
return hands_observation, (base_pos, base_orn)
return None
if __name__ == '__main__':
import time
env = HandObjectEnv()
env.show_window()
env.reset(model_path='duck_vhacd.urdf', up_axis='y')
while pb.isConnected():
time.sleep(0.1)
def run_sim(inp):
goal_pos = np.copy(inp)
angle = 0.
if not p.isConnected():
if not B_VISUALIZE:
p.connect(p.DIRECT)
else:
p.connect(p.GUI)
p.resetDebugVisualizerCamera(cameraDistance=2.0,
cameraYaw=180 + 45,
cameraPitch=-15,
cameraTargetPosition=[0.5, 0.5, 0.6])
p.resetSimulation()
p.setGravity(0, 0, -9.8)
p.setPhysicsEngineParameter(fixedTimeStep=SimConfig.CONTROLLER_DT,
numSubSteps=SimConfig.N_SUBSTEP)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
robot = p.loadURDF(
cwd + "/robot_model/draco3/draco3_gripper_mesh_updated.urdf",
SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT,
SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT)
p.loadURDF(cwd + "/robot_model/ground/plane.urdf", [0, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config(
robot, SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT,
SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT, False)
if B_VISUALIZE:
lh_target_frame = p.loadURDF(cwd + "/robot_model/etc/ball.urdf",
[0., 0, 0.], [0, 0, 0, 1])
lh_target_pos = np.array([0., 0., 0.])
lh_target_quat = np.array([0., 0., 0., 1.])
lh_waypoint_frame = p.loadURDF(cwd + "/robot_model/etc/ball.urdf",
[0., 0, 0.], [0, 0, 0, 1])
lh_waypoint_pos = np.array([0., 0., 0.])
lh_waypoint_quat = np.array([0., 0., 0., 1.])
# Add Gear constraint
c = p.createConstraint(robot,
link_id['l_knee_fe_lp'],
robot,
link_id['l_knee_fe_ld'],
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10)
c = p.createConstraint(robot,
link_id['r_knee_fe_lp'],
robot,
link_id['r_knee_fe_ld'],
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=500, erp=10)
# Initial Config
set_initial_config(robot, joint_id)
# Link Damping
pybullet_util.set_link_damping(robot, link_id.values(), 0., 0.)
# Joint Friction
pybullet_util.set_joint_friction(robot, joint_id, 0.)
gripper_attached_joint_id = OrderedDict()
gripper_attached_joint_id["l_wrist_pitch"] = joint_id["l_wrist_pitch"]
gripper_attached_joint_id["r_wrist_pitch"] = joint_id["r_wrist_pitch"]
pybullet_util.set_joint_friction(robot, gripper_attached_joint_id, 0.1)
# Construct Interface
interface = DracoManipulationInterface()
# Run Sim
t = 0
dt = SimConfig.CONTROLLER_DT
count = 0
nominal_sensor_data = pybullet_util.get_sensor_data(
robot, joint_id, link_id, pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
if B_VISUALIZE:
pybullet_util.draw_link_frame(robot,
link_id['l_hand_contact'],
text="lh")
pybullet_util.draw_link_frame(lh_target_frame, -1, text="lh_target")
pybullet_util.draw_link_frame(lh_waypoint_frame, -1)
gripper_command = dict()
for gripper_joint in GRIPPER_JOINTS:
gripper_command[gripper_joint] = nominal_sensor_data['joint_pos'][
gripper_joint]
waiting_command = True
time_command_recved = 0.
while (1):
# Get SensorData
sensor_data = pybullet_util.get_sensor_data(robot, joint_id, link_id,
pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
for gripper_joint in GRIPPER_JOINTS:
del sensor_data['joint_pos'][gripper_joint]
del sensor_data['joint_vel'][gripper_joint]
rf_height = pybullet_util.get_link_iso(robot,
link_id['r_foot_contact'])[2, 3]
lf_height = pybullet_util.get_link_iso(robot,
link_id['l_foot_contact'])[2, 3]
sensor_data['b_rf_contact'] = True if rf_height <= 0.01 else False
sensor_data['b_lf_contact'] = True if lf_height <= 0.01 else False
if t >= WalkingConfig.INIT_STAND_DUR + 0.1 and waiting_command:
global_goal_pos = np.copy(goal_pos)
global_goal_pos[0] += sensor_data['base_com_pos'][0]
global_goal_pos[1] += sensor_data['base_com_pos'][1]
lh_target_pos = np.copy(global_goal_pos)
lh_target_rot = np.dot(RIGHTUP_GRIPPER, x_rot(angle))
lh_target_quat = util.rot_to_quat(lh_target_rot)
lh_target_iso = liegroup.RpToTrans(lh_target_rot, lh_target_pos)
lh_waypoint_pos = generate_keypoint(lh_target_iso)[0:3, 3]
interface.interrupt_logic.lh_target_pos = lh_target_pos
interface.interrupt_logic.lh_waypoint_pos = lh_waypoint_pos
interface.interrupt_logic.lh_target_quat = lh_target_quat
interface.interrupt_logic.b_interrupt_button_one = True
waiting_command = False
time_command_recved = t
command = interface.get_command(copy.deepcopy(sensor_data))
if B_VISUALIZE:
p.resetBasePositionAndOrientation(lh_target_frame, lh_target_pos,
lh_target_quat)
p.resetBasePositionAndOrientation(lh_waypoint_frame,
lh_waypoint_pos, lh_target_quat)
# Exclude Knee Proximal Joints Command
del command['joint_pos']['l_knee_fe_jp']
del command['joint_pos']['r_knee_fe_jp']
del command['joint_vel']['l_knee_fe_jp']
del command['joint_vel']['r_knee_fe_jp']
del command['joint_trq']['l_knee_fe_jp']
del command['joint_trq']['r_knee_fe_jp']
# Apply Command
pybullet_util.set_motor_trq(robot, joint_id, command['joint_trq'])
pybullet_util.set_motor_pos(robot, joint_id, gripper_command)
p.stepSimulation()
t += dt
count += 1
## Safety On the run
safe_list = is_safe(robot, link_id, sensor_data)
if all(safe_list):
pass
else:
safe_list.append(False)
del interface
break
## Safety at the end
if t >= time_command_recved + ManipulationConfig.T_REACHING_DURATION + 0.2:
if is_tracking_error_safe(global_goal_pos, angle, robot, link_id):
safe_list.append(True)
else:
safe_list.append(False)
del interface
break
return safe_list
def main():
while (p.isConnected()):
time.sleep(1. / 40.) # set to 40 fps
p.stepSimulation()
print('input error, exit')
sys.exit(1)
########################################################
arm = XArmAPI(ip)
arm.motion_enable(enable=True)
arm.set_mode(0)
arm.set_state(state=0)
arm.reset(wait=True)
arm.set_mode(1)
arm.set_state(state=0)
speed = math.radians(5)
while (p.isConnected()):
p.stepSimulation()
targetPositions=[]
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
p.setJointMotorControl2(xarm, jointIds[i], p.POSITION_CONTROL, targetPos, force=5 * 240.)
targetPositions.append(targetPos)
skip_cam_frames -= 1
arm.set_servo_angle_j(angles=targetPositions, speed=speed, is_radian=True)
if (skip_cam_frames<0):
p.getCameraImage(320,200, renderer=p.ER_BULLET_HARDWARE_OPENGL )
skip_cam_frames = 20
time.sleep(1./240.)
p.connect(p.GUI)
p.setGravity(0, 0, -10)
p.loadURDF("data/plane.urdf", [0, 0, 0], useFixedBase=True)
rbt = p.loadURDF("data/balance_car.urdf", [0, 0, 2])
maxForce = 50
c = Controller()
f = 0
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "log.mp4")
while (p.isConnected() and f < 2400):
pos, quat = p.getBasePositionAndOrientation(rbt)
lv, av = p.getBaseVelocity(rbt)
v1, v2 = c.calcWheelVelsAngle(pos, quat, lv, av)
p.setJointMotorControl2(bodyUniqueId=rbt,
jointIndex=0,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=-v1,
force=maxForce)
p.setJointMotorControl2(bodyUniqueId=rbt,
jointIndex=1,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=v2,
force=maxForce)
p.stepSimulation()
time.sleep(1. / 240.)
import pybullet as p
import pybullet_data
import utils
useMaximalCoordinates = False
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
pole = p.loadURDF("cartpole.urdf", [0, 0, 0], useMaximalCoordinates=useMaximalCoordinates)
for i in range(2):
p.setJointMotorControl2(pole, i, p.POSITION_CONTROL, targetPosition=0, force=500)
for i in range(2):
p.resetJointState(pole, i, 0.3, targetVelocity=0.3)
timeStep = 0.0000001
p.setTimeStep(timeStep)
while p.isConnected():
position, velocity, _, _ = p.getJointState(pole, 0)
print(velocity)
p.stepSimulation()
time.sleep(3)
p.setRealTimeSimulation(1)
# use the PyBullet_data package
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0, 0, -10)
# load plane
planeId = p.loadURDF("plane.urdf")
# load frame
frame1_orn = p.getQuaternionFromEuler([0, 0, 0])
frame1_pos = [1, 1, 1]
frame1 = p.loadURDF("./data/frame.urdf",
basePosition=frame1_pos,
baseOrientation=frame1_orn,
useFixedBase=1)
frame2_orn = p.getQuaternionFromEuler([0, 0, 1.57])
frame2_pos = [1, 2, 1]
frame2 = p.loadURDF("./data/frame.urdf",
basePosition=frame2_pos,
baseOrientation=frame2_orn,
useFixedBase=1)
while p.isConnected(physicsClient):
pass
p.disconnect()
