Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
# double m_kDP; // Damping coefficient [0,1]
# double m_kDG; // Drag coefficient [0,+inf]
# double m_kLF; // Lift coefficient [0,+inf]
# double m_kPR; // Pressure coefficient [-inf,+inf]
# double m_kVC; // Volume conversation coefficient [0,+inf]
# double m_kDF; // Dynamic friction coefficient [0,1]
# double m_kMT; // Pose matching coefficient [0,1]
# double m_kCHR; // Rigid contacts hardness [0,1]
# double m_kKHR; // Kinetic contacts hardness [0,1]
# double m_kSHR; // Soft contacts hardness [0,1]
# double m_kAHR; // Anchors hardness [0,1]
# int m_viterations; // Velocities solver iterations
# int m_piterations; // Positions solver iterations
# int m_diterations; // Drift solver iterations
p.setGravity(0, 0, -9.81/2, physicsClientId=self.id) # Let the cloth settle more gently
self.robot.set_gravity(0, 0, 0)
self.human.set_gravity(0, 0, -1)
self.cloth_attachment.set_gravity(0, 0, 0)
p.setPhysicsEngineParameter(numSubSteps=8, physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id)
# Wait for the cloth to settle
for _ in range(50):
# Force the end effector attachment to stay at the end effector
self.cloth_attachment.set_base_pos_orient(self.start_ee_pos, [0, 0, 0, 1])
p.stepSimulation(physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
# in the low dimensional case obs space for problem is (R, 2, 7)
# R = number of repeats
# 2 = two items; cart & pole
# 7d tuple for pos + orientation pose
self.state_shape = (self.repeats, 2, 7)
# check reward type
assert opts.reward_calc in ['fixed', 'angle', 'action', 'angle_action']
self.reward_calc = opts.reward_calc
# no state until reset.
self.state = np.empty(self.state_shape, dtype=np.float32)
# setup bullet
p.connect(p.GUI if self.gui else p.DIRECT)
p.setGravity(0, 0, -9.81)
p.loadURDF("envs/models/ground.urdf", 0,0,0, 0,0,0,1)
self.cartpole = p.loadURDF('envs/models/cartpolev0.urdf',0,0,0)
p.setJointMotorControl2(self.cartpole,0, controlMode=p.VELOCITY_CONTROL, force=0) # turn off active motor/ turn it to 0 force
p.loadURDF("boston_box.urdf",[-2,3,-2], useFixedBase=True)
p.resetDebugVisualizerCamera( cameraDistance=1, cameraYaw=148, cameraPitch=-9, cameraTargetPosition=[0.36,5.3,-0.62])
p.loadURDF("boston_box.urdf",[0,3,-2],useFixedBase=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.getCameraImage(320,200)#, renderer=p.ER_BULLET_HARDWARE_OPENGL )
t=0
p.setRealTimeSimulation(1)
while (1):
p.setGravity(0,0,-10)
time.sleep(0.01)
t+=0.01
keys = p.getKeyboardEvents()
for k in keys:
if (keys[k]&p.KEY_WAS_TRIGGERED):
if (k == ord('i')):
x = 10.*math.sin(t)
y = 10.*math.cos(t)
p.getCameraImage(320,200,lightDirection=[x,y,10],shadow=1)#, renderer=p.ER_BULLET_HARDWARE_OPENGL )
import os
import math
from datetime import datetime
import pybullet_data
import pybullet as p
p.connect(p.GUI)
urdf_root_path = pybullet_data.getDataPath()
p.loadURDF(os.path.join(urdf_root_path, "plane.urdf"), [0, 0, -0.3], useFixedBase=True)
button_uid = p.loadURDF("/urdf/simple_button.urdf", [0, 0, 0])
glider_idx = 1
button_link_idx = 1
p.setGravity(0, 0, -10)
t = 0
useSimulation = True
useRealTimeSimulation = False
p.setRealTimeSimulation(useRealTimeSimulation)
button_pos_slider = p.addUserDebugParameter("button_pos", 0, 1, 0.1)
while True:
if useRealTimeSimulation:
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.1
"""
test = self._test
if not self._urdf_list: # Load from procedural random objects.
if not self._object_filenames:
self._object_filenames = self._get_random_objects(
num_objects=self._num_objects,
test=test,
replace=self._allow_duplicate_objects,
)
self._urdf_list = self._object_filenames
logging.info('urdf_list %s', self._urdf_list)
pybullet.resetSimulation(physicsClientId=self.cid)
pybullet.setPhysicsEngineParameter(
numSolverIterations=150, physicsClientId=self.cid)
pybullet.setTimeStep(self._time_step, physicsClientId=self.cid)
pybullet.setGravity(0, 0, -10, physicsClientId=self.cid)
plane_path = os.path.join(self._urdf_root, 'plane.urdf')
pybullet.loadURDF(plane_path, [0, 0, -1], physicsClientId=self.cid)
table_path = os.path.join(self._urdf_root, 'table/table.urdf')
pybullet.loadURDF(
table_path, [0.5, 0.0, -.82], [0., 0., 0., 1.],
physicsClientId=self.cid)
self._kuka = kuka.Kuka(
urdfRootPath=self._urdf_root,
timeStep=self._time_step,
clientId=self.cid)
self._block_uids = []
for urdf_name in self._urdf_list:
xpos = 0.4 + self._block_random * random.random()
ypos = self._block_random * (random.random() - .5)
angle = np.pi / 2 + self._block_random * np.pi * random.random()
ori = pybullet.getQuaternionFromEuler([0, 0, angle])
def setup(self):
'''
Create task by adding objects to the scene, including the robot.
'''
rospack = rospkg.RosPack()
path = rospack.get_path('costar_simulation')
static_plane_path = os.path.join(path, 'meshes', 'world', 'plane.urdf')
pb.loadURDF(static_plane_path)
self.world = self.makeWorld()
self.task = self._makeTask()
self._setup()
handle = self.robot.load()
pb.setGravity(0, 0, -9.807)
self._setupRobot(handle)
state = self.robot.getState()
self.world.addActor(SimulationRobotActor(
robot=self.robot,
dynamics=SimulationDynamics(self.world),
policy=NullPolicy(),
state=state))
self._updateWorld()
for handle, (obj_type, obj_name) in self._type_and_name_by_obj.items():
# Create an object and add it to the World
state = GetObjectState(handle)
self.world.addObject(obj_name, obj_type, handle, state)
p.loadURDF(os.path.join(self._urdf_root, "plane.urdf"), [0, 0, -1])
self.table_uid = p.loadURDF(os.path.join(self._urdf_root, "table/table.urdf"), 0.5000000, 0.00000, -.820000,
0.000000, 0.000000, 0.0, 1.0)
# Initialize button position
x_pos = 0.5
y_pos = 0
if BUTTON_RANDOM:
x_pos += 0.15 * self.np_random.uniform(-1, 1)
y_pos += 0.3 * self.np_random.uniform(-1, 1)
self.button_uid = p.loadURDF("/urdf/simple_button.urdf", [x_pos, y_pos, Z_TABLE])
self.button_pos = np.array([x_pos, y_pos, Z_TABLE])
p.setGravity(0, 0, -10)
self._kuka = kuka.Kuka(urdf_root_path=self._urdf_root, timestep=self._timestep,
use_inverse_kinematics=(not self.action_joints), small_constraints=(not BUTTON_RANDOM))
self._env_step_counter = 0
# Close the gripper and wait for the arm to be in rest position
for _ in range(500):
if self.action_joints:
self._kuka.applyAction(list(np.array(self._kuka.joint_positions)[:7]) + [0, 0])
else:
self._kuka.applyAction([0, 0, 0, 0, 0])
p.stepSimulation()
# Randomize init arm pos: take 5 random actions
for _ in range(N_RANDOM_ACTIONS_AT_INIT):
if self._is_discrete:
action = [0, 0, 0, 0, 0]
sign = 1 if self.np_random.rand() > 0.5 else -1
import pybullet as p
import pybullet_data as pd
import math
import time
import numpy as np
import panda_sim_grasp as panda_sim
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP,1)
p.setAdditionalSearchPath(pd.getDataPath())
timeStep=1./120.#240.
p.setTimeStep(timeStep)
p.setGravity(0,-9.8,0)
panda = panda_sim.PandaSimAuto(p,[0,0,0])
logId = panda.bullet_client.startStateLogging(panda.bullet_client.STATE_LOGGING_PROFILE_TIMINGS, "log.json")
panda.bullet_client.submitProfileTiming("start")
for i in range (100000):
panda.bullet_client.submitProfileTiming("full_step")
panda.step()
p.stepSimulation()
time.sleep(timeStep)
panda.bullet_client.submitProfileTiming()
panda.bullet_client.submitProfileTiming()
panda.bullet_client.stopStateLogging(logId)
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot, "plane.urdf"), [0, 0, -1])
p.loadURDF(os.path.join(self._urdfRoot, "table/table.urdf"), 0.5000000, 0.00000, -.820000, 0.000000, 0.000000,
0.0, 1.0)
xpos = 0.55 + 0.12 * random.random()
ypos = 0 + 0.2 * random.random()
ang = 3.14 * 0.5 + 3.1415925438 * random.random()
orn = p.getQuaternionFromEuler([0, 0, ang])
self.blockUid = p.loadURDF(os.path.join(self._urdfRoot, "block.urdf"), xpos, ypos, -0.15, orn[0], orn[1],
orn[2], orn[3])
p.setGravity(0, 0, -10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)