def __init__(self,
width=400,
height=300,
fov=60,
pos=[.5, .5, .5],
look_at=[0, 0, 0]):
self.width = width
self.heigt = height
self.view = p.computeViewMatrix(cameraEyePosition=pos,
cameraTargetPosition=look_at,
cameraUpVector=[0, 0, 1])
self.proj = p.computeProjectionMatrixFOV(fov=fov,
aspect=width / height,
nearVal=0.1,
farVal=0.8)
# if this should ever trigger, the official PyPi version (pip install pybullet) is compiled with Numpy headers
assert p.isNumpyEnabled(
) # because if not, copying the pixels of the camera image from C++ to python takes forever
def continuous_torque(self, action):
t1, t2 = action[0]
t1 = 4. * t1
t2 = 20. * t2
t1 = t1 + np.sign(t1) * 30.
t2 = t2 + np.sign(t2) * 30.
return [t1, t2]
# play one step
def step(self, action):
if self.continuous:
torque = self.continuous_torque(action)
else:
torque = self.torque_from_action(action)
p.setJointMotorControlArray(self.unicycleUid,
self.jointinfo,
controlMode=p.TORQUE_CONTROL,
forces=torque)
# should add print angular velocity(acceleration) for given torque depends on time_step
p.stepSimulation()
state, done, reward = self.get_state()
return state, reward, done, [] # last return slot for debug
if __name__ == '__main__':
print(p.isNumpyEnabled())
import pybullet as p
import pybullet_data
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
p.changeVisualShape(ground,-1,rgbaColor=[1,1,1,0.8])
#p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
print("hasNumpy = ",p.isNumpyEnabled())
anymal = p.loadURDF("quadruped/ANYmal/robot.urdf",[3,3,3], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES, useMaximalCoordinates=False)
p.resetSimulation()
ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
#todo, tweak this value to trade solver quality versus performance
p.setPhysicsEngineParameter(solverResidualThreshold=1e-2)
index = 0
numX = 3
numY = 3
for i in range (numX):
for j in range (numY):
def __init__(self,
use_gui,
simulation_config_filename,
cad_models_additional_search_path,
static_entities_config_filename,
robot_urdf_file_path,
global_frame_id,
base_frame_id,
load_robot=True):
""" Internal simulator constructor
"""
if simulation_config_filename != "":
with open(simulation_config_filename, 'r') as config:
self.simulator_config = yaml.safe_load(config)
else:
self.load_robot = False
self.tf_bridge = TfBridge()
self.nodes_map = {}
self.my_id = None
self.entity_id_map = {}
self.reverse_entity_id_map = {}
self.joint_id_map = {}
self.reverse_joint_id_map = {}
self.constraint_id_map = {}
self.markers_id_map = {}
self.robot_joints_command = []
self.robot_joints_command_indices = []
self.position_tolerance = self.simulator_config["base_config"]["position_tolerance"]
if "controller_config" in self.simulator_config:
self.use_controller = True
else:
self.use_controller = False
self.global_frame_id = global_frame_id
self.base_frame_id = base_frame_id
self.robot_urdf_file_path = robot_urdf_file_path
self.robot_moving = False
if not p.isNumpyEnabled():
rospy.logwarn("Numpy is not enabled, rendering can be slow")
self.use_gui = use_gui
if self.use_gui is True:
self.client_simulator_id = p.connect(p.GUI)
else:
self.client_simulator_id = p.connect(p.DIRECT)
if cad_models_additional_search_path != "":
p.setAdditionalSearchPath(cad_models_additional_search_path)
self.static_nodes = []
self.not_static_nodes = []
if static_entities_config_filename != "":
with open(static_entities_config_filename, 'r') as stream:
static_entities = yaml.safe_load(stream)
for entity in static_entities:
start_pose = Vector6D(x=entity["position"]["x"],
y=entity["position"]["y"],
z=entity["position"]["z"],
rx=entity["orientation"]["x"],
ry=entity["orientation"]["x"],
rz=entity["orientation"]["z"])
success, static_node = self.load_urdf(entity["file"],
start_pose,
id=entity["id"],
label=entity["label"],
description=entity["description"],
static=True)
if not success:
rospy.logwarn("[simulator] Unable to load {} node, skip it.".format(entity["id"]))
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
self.robot_loaded = False
self.joint_states_last_update = None
if load_robot is True:
self.joint_state_subscriber = rospy.Subscriber("/joint_states", JointState, self.joint_states_callback, queue_size=1)
if self.use_controller is True:
pass
#leafID1 = p.loadURDF(leaf_path, leafStartPos1, leafStartOrn1, useFixedBase=True,flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
#leafID2 = p.loadURDF(leaf_path, leafStartPos2, leafStartOrn2, useFixedBase=True,flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
#p.changeVisualShape(leafID1, -1, textureUniqueId=textureId_leaf)
#p.changeVisualShape(leafID2, -1, textureUniqueId=textureId_leaf)
# apple2 = p.loadURDF(apple_Path, appleStartPos2, appleStartOrn2,useFixedBase=True, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
# cube = p.loadURDF("cube_small.urdf", robotStartPos1, robotStartOrn1)
#sphereID = p.loadURDF(sphere_Path, sphereStartPos1, sphereStartOrn1,useFixedBase=True, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES)
# physicsClient.addUserData(planeID, "MyKey1", "MyValue1")
eefID = 7
x_init = 0.1
y_init = 0.15 #0.15
z_init = 0.7
roll_init = 0.2
yaw_init = -0.1
pitch_init = 0 #0
isnum = p.isNumpyEnabled()
print('===Numpy Enable=====')
print('Is numpy enabled = ', isnum)
ikp.pos(x_init, y_init, z_init, roll_init, yaw_init, pitch_init, robotID,
eefID)
flag = True
xin = p.addUserDebugParameter("x", -2, 2, 0)
yin = p.addUserDebugParameter("y", -2, 2, 0)
zin = p.addUserDebugParameter("z", -2, 2, 0)
roll1 = p.addUserDebugParameter("roll", -pi, pi, 0)
yaw1 = p.addUserDebugParameter("yaw", -pi, pi, 0)
pitch1 = p.addUserDebugParameter("pitch", -pi, pi, 0)
xc = 0
yc = 0
zc = 0
import time
import numpy as np
import pybullet
from pybullet_utils import bullet_client
from vgn.utils.transform import Rotation, Transform
assert pybullet.isNumpyEnabled(), "Pybullet needs to be built with NumPy"
class BtWorld(object):
"""Interface to a PyBullet physics server.
Attributes:
dt: Time step of the physics simulation.
rtf: Real time factor. If negative, the simulation is run as fast as possible.
sim_time: Virtual time elpased since the last simulation reset.
"""
def __init__(self, gui=True):
connection_mode = pybullet.GUI if gui else pybullet.DIRECT
self.p = bullet_client.BulletClient(connection_mode)
self.gui = gui
self.dt = 1.0 / 240.0
self.solver_iterations = 150
self.reset()
def __init__(self):
rospy.loginfo("Subscribing to /tf topic...")
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
# set the parameters
self.env_sdf_file_path = rospy.get_param("~env_sdf_file_path", "")
self.robot_urdf_file_path = rospy.get_param("~robot_urdf_file_path",
"r2d2.urdf")
self.base_frame_id = rospy.get_param("~base_frame_id", "base_link")
self.global_frame_id = rospy.get_param("~global_frame_id", "map")
self.objects_cad_models_dir = rospy.get_param(
"~objects_cad_models_dir", "")
self.robot_cad_models_dir = rospy.get_param("~robot_cad_models_dir",
"")
self.position_tolerance = rospy.get_param("~position_tolerance", 0.01)
self.velocity_tolerance = rospy.get_param("~velocity_tolerance", 0.001)
self.camera_info_topic = rospy.get_param("~rgb_camera_info_topic",
"/camera/rgb/camera_info")
self.time_step = rospy.get_param("~time_step", 1 / 240.0)
# Initialize attributes
self.simulator_entity_id_map = {}
self.reverse_simulator_entity_id_map = {}
self.simulator_joint_id_map = {}
self.reverse_simulator_joint_id_map = {}
self.entities_state = {}
self.entities_last_update = {}
self.camera_frame_id = None
self.last_backup_id = None
self.camera_info = None
self.last_observation_time = None
self.average_observation_duration = None
self.bridge = CvBridge()
self.visualization_publisher = rospy.Publisher(
"uwds3_core/debug_image", sensor_msgs.msg.Image, queue_size=1)
# Setup the physics server
use_gui = rospy.get_param("~use_gui", True)
if use_gui is True:
self.physics = p.connect(p.GUI)
else:
self.physics = p.connect(p.DIRECT)
if p.isNumpyEnabled() is not True:
rospy.logwarn("Numpy is not enabled in pybullet !")
# Add search path
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Load the ground
self.simulator_entity_id_map[self.global_frame_id] = p.loadURDF(
"plane.urdf")
# if self.robot_cad_models_dir != "":
# p.setAdditionalSearchPath(self.robot_cad_models_dir)
if self.objects_cad_models_dir != "":
p.setAdditionalSearchPath(self.objects_cad_models_dir)
# Load the environment if any
if self.env_sdf_file_path != "":
self.simulator_entity_id_map["env"] = p.loadURDF(
self.env_sdf_file_path)
self.robot_loaded = False
# Set the gravity
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
# TODO
#self.simulation_timer =
# Subscribe to joint state message
rospy.loginfo("Subscribing to /joint_states topic...")
self.joint_state_subscriber = rospy.Subscriber(
"/joint_states", sensor_msgs.msg.JointState,
self.joint_states_callback)
self.camera_info_topic = rospy.get_param("~camera_info_topic",
"/camera/rgb/camera_info")
rospy.loginfo("Subscribing to /{} topic...".format(
self.camera_info_topic))
self.camera_info_received = False
self.camera_info_subscriber = rospy.Subscriber(
self.camera_info_topic, sensor_msgs.msg.CameraInfo,
self.camera_info_callback)
from simulation_issue import Simulation
from argparse import ArgumentParser
import pybullet
if not pybullet.isNumpyEnabled():
raise RuntimeError(
"PyBullet must have been compiled with NumPy support. Try upgradinng it or install from source."
)
'''
Drift demonstration script
It creates a simulator object in a thread and runs. During running, the
motor of the gantry is accelerated for n cycles and after reaching the
threshold it is de-accelerated. After stopping, there should be a drift
visible in the GUI. The drift does not stop, despite the fact,
friction is enabled.
'''
def main():
parser = ArgumentParser()
parser.add_argument(
"-g",
"--gui",
dest="gui",
action="store_true",
default=True,