def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
#for i in range (p.getNumJoints(self.kukaUid)):
# print(p.getJointInfo(self.kukaUid,i))
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
for i in range (self.numJoints):
jointInfo = p.getJointInfo(self.kukaUid,i)
qIndex = jointInfo[3]
if qIndex > -1:
#print("motorname")
#print(jointInfo[1])
self.motorNames.append(str(jointInfo[1]))
self.motorIndices.append(i)
def robot_specific_reset(self): HumanoidFlagrun.robot_specific_reset(self) self.frame = 0 if (self.aggressive_cube): p.resetBasePositionAndOrientation(self.aggressive_cube.bodies[0],[-1.5,0,0.05],[0,0,0,1]) else: self.aggressive_cube = get_cube(-1.5,0,0.05) self.on_ground_frame_counter = 0 self.crawl_start_potential = None self.crawl_ignored_potential = 0.0 self.initial_z = 0.8
def Step(stepIndex): for objectId in range(objectNum): record = log[stepIndex*objectNum+objectId] Id = record[2] pos = [record[3],record[4],record[5]] orn = [record[6],record[7],record[8],record[9]] p.resetBasePositionAndOrientation(Id,pos,orn) numJoints = p.getNumJoints(Id) for i in range (numJoints): jointInfo = p.getJointInfo(Id,i) qIndex = jointInfo[3] if qIndex > -1: p.resetJointState(Id,i,record[qIndex-7+17])
def flag_reposition(self):
self.walk_target_x = self.np_random.uniform(low=-self.scene.stadium_halflen, high=+self.scene.stadium_halflen)
self.walk_target_y = self.np_random.uniform(low=-self.scene.stadium_halfwidth, high=+self.scene.stadium_halfwidth)
more_compact = 0.5 # set to 1.0 whole football field
self.walk_target_x *= more_compact
self.walk_target_y *= more_compact
if (self.flag):
#for b in self.flag.bodies:
# print("remove body uid",b)
# p.removeBody(b)
p.resetBasePositionAndOrientation(self.flag.bodies[0],[self.walk_target_x, self.walk_target_y, 0.7],[0,0,0,1])
else:
self.flag = get_sphere(self.walk_target_x, self.walk_target_y, 0.7)
self.flag_timeout = 600/self.scene.frame_skip #match Roboschool
def testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
urdfs = [
"TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
"kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
]
for urdf in urdfs:
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
robotId = p.loadURDF(urdf, useFixedBase=True)
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
numJoints = p.getNumJoints(robotId)
endEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
p.stepSimulation()
# Get the joint and link state directly from Bullet.
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
result = p.getLinkState(robotId,
endEffectorIndex,
computeLinkVelocity=1,
computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
# The localPosition is always defined in terms of the link frame coordinates.
zero_vec = [0.0] * len(mpos)
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
zero_vec)
assert (allclose(dot(jac_t, mvel), link_vt))
assert (allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
def reset(self): p.resetBasePositionAndOrientation(self.pybullet_id, self.initial_position, (0., 0., 0., 1.))
import time
import glob
import common.gym_interface as gym_interface
import pybullet as p
import os
import pybullet_data
import shutil
import re
p.connect(p.GUI)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
files = glob.glob("../input_data/bodies/5*.xml")
files.sort()
print(files)
p.resetSimulation()
filename = os.path.join(pybullet_data.getDataPath(), "plane_stadium.sdf")
_ = p.loadSDF(filename)
robots = [None] * len(files)
for i in range(len(files)):
(robots[i], ) = p.loadMJCF(files[i])
p.resetBasePositionAndOrientation(robots[i], [i * 2, 0, 1], [0, 0, 0, 1])
while True:
p.stepSimulation()
time.sleep(0.01)
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
import pybullet as p
import time
import math
from datetime import datetime
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -0.3])
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints != 7):
exit()
#lower limits for null space
ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space
jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
for i in range(numJoints):
p.resetJointState(kukaId, i, rp[i])
p.setGravity(0, 0, 0)
t = 0.
import pybullet as p
import time
import math
from datetime import datetime
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.GUI)
p.loadURDF("plane.urdf", [0, 0, -1.3])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sawyerId = p.loadURDF("pole.urdf", [0, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
sawyerEndEffectorIndex = 3
numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents
jd = [0.1, 0.1, 0.1, 0.1]
p.setGravity(0, 0, 0)
t = 0.
prevPose = [0, 0, 0]
prevPose1 = [0, 0, 0]
hasPrevPose = 0
ikSolver = 0
useRealTimeSimulation = 0
p.setRealTimeSimulation(useRealTimeSimulation)
#trailDuration is duration (in seconds) after debug lines will be removed automatically
#use 0 for no-removal
trailDuration = 1
#time.sleep(1.0)
#raw_input("Press Enter to continue...")
for i in range(20000):
for j in range(3):
p.resetJointStateMultiDof(human_adult_ID, j,
p.getQuaternionFromEuler([0, 0, 0]))
p.resetJointStateMultiDof(
human_adult_ID, 3,
p.getQuaternionFromEuler([i / 100.0, i / 150.0, i / 350.0]))
#for j in range(p.getNumJoints(human_adult_ID)):
# p.resetJointStateMultiDof(human_adult_ID,
# j,
# targetValue=[i/10.0,i/8.0,i/10.0],
# targetVelocity=[0.01,0.01,0.01])
p.resetBasePositionAndOrientation(human_adult_ID, [0.1, 0.1, 0.1],
p.getQuaternionFromEuler([0, 0, 0]))
p.stepSimulation()
time.sleep(1. / 240.)
#p.getNumJoints(humanoid)
#p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
#for i in range(2000):
# pos, ori = p.getBasePositionAndOrientation(human_adult_ID)
# p.applyExternalForce(human_adult_ID, -1,
# [50*math.cos(i/100.0*math.pi), 50*math.sin(i/88.0*math.pi), 50*math.sin(i/130.0*math.pi)],
# pos, p.WORLD_FRAME)
# p.stepSimulation()
# time.sleep(1./240.)
p.disconnect()
import pybullet as p
import time
p.connect(p.DIRECT)
p.setGravity(0, 0, -10)
p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1. / 240.)
p.setPhysicsEngineParameter(numSubSteps=1)
p.loadURDF("plane.urdf")
objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob, [0.789351, 0.962124, 0.113124],
[0.710965, 0.218117, 0.519402, -0.420923])
jointPositions = [
-0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119,
-0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698,
0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217,
0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374,
0.000000
]
for jointIndex in range(p.getNumJoints(ob)):
p.resetJointState(ob, jointIndex, jointPositions[jointIndex])
#first let the humanoid fall
#p.setRealTimeSimulation(1)
#time.sleep(5)
p.setRealTimeSimulation(0)
#p.saveWorld("lyiing.py")
#now do a benchmark
def set_position_orientation(self, pos, orn):
p.resetBasePositionAndOrientation(self.body_id, pos, orn)
def reset(self):
self.setup_timing()
self.task_success = 0
self.contact_points_on_arm = {}
self.human, self.bed, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(
furniture_type='bed',
static_human_base=False,
human_impairment='random',
print_joints=False,
gender='random')
self.robot_lower_limits = self.robot_lower_limits[
self.robot_left_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[
self.robot_left_arm_joint_indices]
self.reset_robot_joints()
friction = 5
p.changeDynamics(self.bed,
-1,
lateralFriction=friction,
spinningFriction=friction,
rollingFriction=friction,
physicsClientId=self.id)
# Setup human in the air and let them settle into a resting pose on the bed
joints_positions = [(3, np.deg2rad(30))]
controllable_joints = []
self.world_creation.setup_human_joints(self.human,
joints_positions,
controllable_joints,
use_static_joints=False,
human_reactive_force=None)
p.resetBasePositionAndOrientation(self.human, [-0.15, 0.2, 0.95],
p.getQuaternionFromEuler(
[-np.pi / 2.0, 0, 0],
physicsClientId=self.id),
physicsClientId=self.id)
p.setGravity(0, 0, -1, physicsClientId=self.id)
# Add small variation in human joint positions
for j in range(p.getNumJoints(self.human, physicsClientId=self.id)):
if p.getJointInfo(self.human, j,
physicsClientId=self.id)[2] != p.JOINT_FIXED:
p.resetJointState(self.human,
jointIndex=j,
targetValue=self.np_random.uniform(
-0.1, 0.1),
targetVelocity=0,
physicsClientId=self.id)
# Let the person settle on the bed
for _ in range(100):
p.stepSimulation(physicsClientId=self.id)
# Lock human joints and set velocities to 0
joints_positions = []
self.human_controllable_joint_indices = [
0, 1, 2, 3, 4, 5, 6, 7, 8, 9
] if self.human_control else []
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices,
use_static_joints=True,
human_reactive_force=None,
human_reactive_gain=0.01)
self.target_human_joint_positions = []
if self.human_control:
human_joint_states = p.getJointStates(
self.human,
jointIndices=self.human_controllable_joint_indices,
physicsClientId=self.id)
self.target_human_joint_positions = np.array(
[x[0] for x in human_joint_states])
self.human_lower_limits = self.human_lower_limits[
self.human_controllable_joint_indices]
self.human_upper_limits = self.human_upper_limits[
self.human_controllable_joint_indices]
p.changeDynamics(self.human, -1, mass=0, physicsClientId=self.id)
p.resetBaseVelocity(self.human,
linearVelocity=[0, 0, 0],
angularVelocity=[0, 0, 0],
physicsClientId=self.id)
p.setGravity(0, 0, 0, physicsClientId=self.id)
p.setGravity(0, 0, -1, body=self.human, physicsClientId=self.id)
# Find the base position and joint positions for a static person in bed
# print(p.getBasePositionAndOrientation(self.human, physicsClientId=self.id))
# joint_states = p.getJointStates(self.human, jointIndices=list(range(p.getNumJoints(self.human, physicsClientId=self.id))), physicsClientId=self.id)
# joint_positions = np.array([x[0] for x in joint_states])
# joint_string = '['
# for i, jp in enumerate(joint_positions):
# joint_string += '(%d, %.4f), ' % (i, jp)
# print(joint_string + ']')
# exit()
shoulder_pos, shoulder_orient = p.getLinkState(
self.human,
5,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human,
7,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human,
9,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
target_pos = np.array([-0.6, 0.2, 1]) + self.np_random.uniform(
-0.05, 0.05, size=3)
if self.robot_type == 'pr2':
target_orient = np.array(
p.getQuaternionFromEuler(np.array([0, 0, 0]),
physicsClientId=self.id))
self.position_robot_toc(
self.robot,
76, [(target_pos, target_orient)], [(shoulder_pos, None),
(elbow_pos, None),
(wrist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=range(29, 29 + 7),
pos_offset=np.array([-0.1, 0, 0]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.world_creation.set_gripper_open_position(self.robot,
position=0.2,
left=True,
set_instantly=True)
self.tool = self.world_creation.init_tool(
self.robot,
mesh_scale=[1] * 3,
pos_offset=[0, 0, 0],
orient_offset=p.getQuaternionFromEuler(
[0, 0, 0], physicsClientId=self.id),
maximal=False)
elif self.robot_type == 'jaco':
target_orient = p.getQuaternionFromEuler(np.array(
[0, np.pi / 2.0, 0]),
physicsClientId=self.id)
base_position, base_orientation, _ = self.position_robot_toc(
self.robot,
8, [(target_pos, target_orient)], [(shoulder_pos, None),
(elbow_pos, None),
(wrist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=[0, 1, 2, 3, 4, 5, 6],
pos_offset=np.array([-0.05, 1.05, 0.6]),
max_ik_iterations=200,
step_sim=True,
random_position=0.1,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.world_creation.set_gripper_open_position(self.robot,
position=1.1,
left=True,
set_instantly=True)
self.tool = self.world_creation.init_tool(
self.robot,
mesh_scale=[1] * 3,
pos_offset=[-0.01, 0, 0.03],
orient_offset=p.getQuaternionFromEuler(
[0, -np.pi / 2.0, 0], physicsClientId=self.id),
maximal=False)
# Load a nightstand in the environment for the jaco arm
self.nightstand_scale = 0.275
visual_filename = os.path.join(self.world_creation.directory,
'nightstand', 'nightstand.obj')
collision_filename = os.path.join(self.world_creation.directory,
'nightstand', 'nightstand.obj')
nightstand_visual = p.createVisualShape(
shapeType=p.GEOM_MESH,
fileName=visual_filename,
meshScale=[self.nightstand_scale] * 3,
rgbaColor=[0.5, 0.5, 0.5, 1.0],
physicsClientId=self.id)
nightstand_collision = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=collision_filename,
meshScale=[self.nightstand_scale] * 3,
physicsClientId=self.id)
nightstand_pos = np.array([-0.9, 0.7, 0]) + base_position
nightstand_orient = p.getQuaternionFromEuler(
np.array([np.pi / 2.0, 0, 0]), physicsClientId=self.id)
self.nightstand = p.createMultiBody(
baseMass=0,
baseCollisionShapeIndex=nightstand_collision,
baseVisualShapeIndex=nightstand_visual,
basePosition=nightstand_pos,
baseOrientation=nightstand_orient,
baseInertialFramePosition=[0, 0, 0],
useMaximalCoordinates=False,
physicsClientId=self.id)
else:
target_orient = p.getQuaternionFromEuler(np.array(
[0, np.pi / 2.0, 0]),
physicsClientId=self.id)
if self.robot_type == 'baxter':
self.position_robot_toc(
self.robot,
48, [(target_pos, target_orient)], [(shoulder_pos, None),
(elbow_pos, None),
(wrist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=range(10, 17),
pos_offset=np.array([-0.2, 0, 0.975]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
else:
self.position_robot_toc(
self.robot,
19, [(target_pos, target_orient)], [(shoulder_pos, None),
(elbow_pos, None),
(wrist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_lower_limits,
self.robot_upper_limits,
ik_indices=[0, 2, 3, 4, 5, 6, 7],
pos_offset=np.array([-0.2, 0, 0.975]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.world_creation.set_gripper_open_position(self.robot,
position=0.0125,
left=True,
set_instantly=True)
self.tool = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.001] * 3,
pos_offset=[0, 0.1175, 0],
orient_offset=p.getQuaternionFromEuler(
[np.pi / 2.0, 0, np.pi / 2.0], physicsClientId=self.id),
maximal=False)
self.generate_targets()
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
return self._get_obs([0], [0, 0])
def get_total_force(self):
total_force = 0
tool_force = 0
tool_force_on_human = 0
total_force_on_human = 0
new_contact_points = 0
for c in p.getContactPoints(bodyA=self.tool, physicsClientId=self.id):
total_force += c[9]
tool_force += c[9]
for c in p.getContactPoints(bodyA=self.robot, physicsClientId=self.id):
bodyB = c[2]
if bodyB != self.tool:
total_force += c[9]
for c in p.getContactPoints(bodyA=self.robot,
bodyB=self.human,
physicsClientId=self.id):
total_force_on_human += c[9]
for c in p.getContactPoints(bodyA=self.tool,
bodyB=self.human,
physicsClientId=self.id):
linkA = c[3]
linkB = c[4]
contact_position = np.array(c[6])
total_force_on_human += c[9]
if linkA in [1]:
tool_force_on_human += c[9]
# Contact with human upperarm, forearm, hand
if linkB < 0 or linkB > p.getNumJoints(
self.human, physicsClientId=self.id):
continue
indices_to_delete = []
for i, (target_pos_world, target) in enumerate(
zip(self.targets_pos_upperarm_world,
self.targets_upperarm)):
if np.linalg.norm(contact_position -
target_pos_world) < 0.025:
# The robot made contact with a point on the person's arm
new_contact_points += 1
self.task_success += 1
p.resetBasePositionAndOrientation(
target, [1000, 1000, 1000], [0, 0, 0, 1],
physicsClientId=self.id)
indices_to_delete.append(i)
self.targets_pos_on_upperarm = [
t for i, t in enumerate(self.targets_pos_on_upperarm)
if i not in indices_to_delete
]
self.targets_upperarm = [
t for i, t in enumerate(self.targets_upperarm)
if i not in indices_to_delete
]
self.targets_pos_upperarm_world = [
t for i, t in enumerate(self.targets_pos_upperarm_world)
if i not in indices_to_delete
]
indices_to_delete = []
for i, (target_pos_world, target) in enumerate(
zip(self.targets_pos_forearm_world,
self.targets_forearm)):
if np.linalg.norm(contact_position -
target_pos_world) < 0.025:
# The robot made contact with a point on the person's arm
new_contact_points += 1
self.task_success += 1
p.resetBasePositionAndOrientation(
target, [1000, 1000, 1000], [0, 0, 0, 1],
physicsClientId=self.id)
indices_to_delete.append(i)
self.targets_pos_on_forearm = [
t for i, t in enumerate(self.targets_pos_on_forearm)
if i not in indices_to_delete
]
self.targets_forearm = [
t for i, t in enumerate(self.targets_forearm)
if i not in indices_to_delete
]
self.targets_pos_forearm_world = [
t for i, t in enumerate(self.targets_pos_forearm_world)
if i not in indices_to_delete
]
return total_force, tool_force, tool_force_on_human, total_force_on_human, new_contact_points
humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1]) humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1]) humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1]) humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1]) startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447, 1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153, 0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495, 0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348] startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-0.1050802848,0,0.935706195,0.08277326387,0.3002461862,0,0,0,0,0,1.114409628,0.3618553952, -0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0, -4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973] p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1]) p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1]) p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1]) p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1]) index0=7 for j in range (p.getNumJoints(humanoid)): ji = p.getJointInfo(humanoid,j) targetPosition=[0] jointType = ji[2] if (jointType == p.JOINT_SPHERICAL): targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]] targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]] index0+=4
1.000000)
]
objects = [
p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.800000, 0.000000, 0.000000, 0.000000,
1.000000)
]
objects = [
p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.900000, 0.000000, 0.000000, 0.000000,
1.000000)
]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0]
print("kuka gripper=")
print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper, [0.923103, -0.200000, 1.250036],
[-0.000000, 0.964531, -0.000002, -0.263970])
jointPositions = [
0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000
]
for jointIndex in range(p.getNumJoints(kuka_gripper)):
p.resetJointState(kuka_gripper, jointIndex, jointPositions[jointIndex])
p.setJointMotorControl2(kuka_gripper, jointIndex, p.POSITION_CONTROL, jointPositions[jointIndex],
0)
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper, 0, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0.05],
[0, 0, 0])
objects = [
p.loadURDF("table/table.urdf", 1.000000, -0.200000, 0.000000, 0.000000, 0.000000, 0.707107,
0.707107)
]
import pybullet as p
import pybullet_data
import time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadSDF("stadium.sdf")
p.setGravity(0, 0, -10)
objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0]
p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1])
p.changeDynamics(sphere, -1, linearDamping=0.9)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch, cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
camForward = camInfo[5]
keys = p.getKeyboardEvents()
import pybullet as p
import time
import math
from datetime import datetime
import pybullet_data
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf", [0, 0, -.98])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
sawyerId = p.loadURDF("rudis_magic_sawyer.urdf", [0, 0, 0])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.resetBasePositionAndOrientation(sawyerId, [0, 0, 0], [0, 0, 0, 1])
### MY CODE
### END OF MY CODE
#bad, get it from name! sawyerEndEffectorIndex = 18
sawyerEndEffectorIndex = 16
numJoints = p.getNumJoints(sawyerId)
#joint damping coefficents
jd = [
0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001,
0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001
]
p.setGravity(0, 0, 0)
t = 0.
prevPose = [0, 0, 0]
def update_feet_tasks(self, k_loop, gait, looping, interface, ftps_Ids_deb):
"""Update the 3D desired position for feet in swing phase by using a 5-th order polynomial that lead them
to the desired position on the ground (computed by the footstep planner)
Args:
k_loop (int): number of time steps since the start of the current gait cycle
pair (int): the current pair of feet in swing phase, for a walking trot gait
looping (int): total number of time steps in one gait cycle
interface (Interface object): interface between the simulator and the MPC/InvDyn
ftps_Ids_deb (list): IDs of debug spheres in PyBullet
"""
# Indexes of feet in swing phase
feet = np.where(gait[0, 1:] == 0)[0]
if len(feet) == 0: # If no foot in swing phase
return 0
t0s = []
for i in feet: # For each foot in swing phase get remaining duration of the swing phase
# Index of the line containing the next stance phase
index = next((idx for idx, val in np.ndenumerate(gait[:, 1+i]) if (((val==1)))), [-1])[0]
remaining_iterations = np.cumsum(gait[:index, 0])[-1] * self.k_mpc - (k_loop % self.k_mpc)
# Compute total duration of current swing phase
i_iter = 1
self.t_swing[i] = gait[0, 0]
while gait[i_iter, 1+i] == 0:
self.t_swing[i] += gait[i_iter, 0]
i_iter += 1
i_iter = -1
while gait[i_iter, 1+i] == 0:
self.t_swing[i] += gait[i_iter, 0]
i_iter -= 1
self.t_swing[i] *= self.dt * self.k_mpc
t0s.append(np.round(self.t_swing[i] - remaining_iterations * self.dt, decimals=3))
# self.footsteps contains the target (x, y) positions for both feet in swing phase
for i in range(len(feet)):
i_foot = feet[i]
# Get desired 3D position, velocity and acceleration
if t0s[i] == 0.000:
[x0, dx0, ddx0, y0, dy0, ddy0, z0, dz0, ddz0, gx1, gy1] = (self.ftgs[i_foot]).get_next_foot(
interface.o_feet[0, i_foot], interface.ov_feet[0, i_foot], interface.oa_feet[0, i_foot],
interface.o_feet[1, i_foot], interface.ov_feet[1, i_foot], interface.oa_feet[1, i_foot],
self.footsteps[0, i_foot], self.footsteps[1, i_foot], t0s[i], self.t_swing[i_foot], self.dt)
self.mgoals[:, i_foot] = np.array([x0, dx0, ddx0, y0, dy0, ddy0])
else:
[x0, dx0, ddx0, y0, dy0, ddy0, z0, dz0, ddz0, gx1, gy1] = (self.ftgs[i_foot]).get_next_foot(
self.mgoals[0, i_foot], self.mgoals[1, i_foot], self.mgoals[2, i_foot],
self.mgoals[3, i_foot], self.mgoals[4, i_foot], self.mgoals[5, i_foot],
self.footsteps[0, i_foot], self.footsteps[1, i_foot], t0s[i], self.t_swing[i_foot], self.dt)
self.mgoals[:, i_foot] = np.array([x0, dx0, ddx0, y0, dy0, ddy0])
# Take into account vertical offset of Pybullet
z0 += interface.mean_feet_z
# Store desired position, velocity and acceleration for later call to this function
self.goals[:, i_foot] = np.array([x0, y0, z0])
self.vgoals[:, i_foot] = np.array([dx0, dy0, dz0])
self.agoals[:, i_foot] = np.array([ddx0, ddy0, ddz0])
# Update desired pos, vel, acc
self.sampleFeet[i_foot].pos(np.matrix([x0, y0, z0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]).T)
self.sampleFeet[i_foot].vel(np.matrix([dx0, dy0, dz0, 0.0, 0.0, 0.0]).T)
self.sampleFeet[i_foot].acc(np.matrix([ddx0, ddy0, ddz0, 0.0, 0.0, 0.0]).T)
# Set reference
self.feetTask[i_foot].setReference(self.sampleFeet[i_foot])
# Update footgoal for display purpose
self.feetGoal[i_foot].translation = np.matrix([x0, y0, z0]).T
# Display the goal position of the feet as green sphere in PyBullet
pyb.resetBasePositionAndOrientation(ftps_Ids_deb[i_foot],
posObj=np.array([gx1, gy1, 0.0]),
ornObj=np.array([0.0, 0.0, 0.0, 1.0]))
return 0
def set_position(self, pos):
_, old_orn = p.getBasePositionAndOrientation(self.body_id)
p.resetBasePositionAndOrientation(self.body_id, pos, old_orn)
def runSim(self, net, weights, runType=pbl.DIRECT):
physicsClient = pbl.connect(runType)
pbl.setAdditionalSearchPath(pybullet_data.getDataPath())
pbl.setGravity(0, 0, -9.8)
ObstacleIDs = []
ObstacleLocations = []
ObstacleOrientations = []
ObstacleIDs.append(
pbl.loadURDF("/Misc/Enclosure/Wall.urdf",
basePosition=[0, -1, 5.5]))
ObstacleIDs.append(
pbl.loadURDF("/Misc/Enclosure/Wall.urdf",
basePosition=[0, 11, 5.5]))
ObstacleIDs.append(
pbl.loadURDF("/Misc/Enclosure/Wall.urdf",
basePosition=[-6, 5, 5.5],
baseOrientation=pbl.getQuaternionFromEuler(
[0, 0, -np.pi / 2])))
ObstacleIDs.append(
pbl.loadURDF("/Misc/Enclosure/Wall.urdf",
basePosition=[6, 5, 5.5],
baseOrientation=pbl.getQuaternionFromEuler(
[0, 0, -np.pi / 2])))
ObstacleIDs.append(
pbl.loadURDF("/Misc/Enclosure/Wall.urdf",
basePosition=[0, 5, 11.5],
baseOrientation=pbl.getQuaternionFromEuler(
[-np.pi / 2, 0, 0])))
[(ObstacleLocations.append(pbl.getBasePositionAndOrientation(obs)[0]),
ObstacleOrientations.append(
pbl.getBasePositionAndOrientation(obs)[1]))
for obs in ObstacleIDs]
dir = np.random.normal(0, 1, size=(3, ))
dir /= np.linalg.norm(dir)
goal = np.array([
np.random.uniform(-4, 4),
np.random.uniform(0, 8),
np.random.uniform(0, 4)
])
start = goal + dir * self.L
ObstacleIDs, ObstacleLocations = self.generateObstacles(
5, start, ObstacleIDs, ObstacleLocations, ObstacleLocations)
Qimmiq = Drone(ObstacleIDs, net=net, init_pos=start, goal=goal)
i = 0
Collided = False
while (not Qimmiq.targetReached) and not Collided and Qimmiq.time < 20:
for o in range(len(Qimmiq.obstacles[:-1])):
if self.hasCollided(Qimmiq, Qimmiq.obstacles[o]):
Collided = True
print('Collision!')
break
[
pbl.resetBasePositionAndOrientation(ObstacleIDs[i],
ObstacleLocations[i],
ObstacleOrientations[i])
for i in range(len(ObstacleOrientations))
]
if i % 240 == 0:
velocities = [
np.random.normal(size=(3, ))
for k in range(len(ObstacleIDs[:-1]))
]
[
pbl.resetBaseVelocity(ObstacleIDs[j], velocities[j], [0, 0, 0])
for j in range(len(ObstacleIDs[len(ObstacleOrientations):-1]))
]
i += 1
Qimmiq.moveDrone(Qimmiq.time_interval, i)
pbl.stepSimulation()
pbl.disconnect()
print("lifeTime: {}".format(Qimmiq.time))
print("Intrusion: {}".format(Qimmiq.Intrusion))
print("ITSE: {}".format(10 * Qimmiq.e))
# return Qimmiq.time
print("Fitness: {0}".format(
-1 * (weights[0] * Qimmiq.e + weights[1] * Qimmiq.Intrusion)))
return -1 * (weights[0] * Qimmiq.e + weights[1] * Qimmiq.Intrusion)
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
p.resetSimulation()
p.setTimeStep(time_step)
p.setGravity(0.0, 0.0, gravity_constant)
p.loadURDF("plane.urdf",[0,0,-0.3])
kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
numJoints = p.getNumJoints(kukaId)
kukaEndEffectorIndex = numJoints - 1
# Set a joint target for the position control and step the sim.
setJointPosition(kukaId, [0.1] * numJoints)
p.stepSimulation()
# Get the joint and link state directly from Bullet.
pos, vel, torq = getJointStates(kukaId)
mpos, mvel, mtorq = getMotorJointStates(kukaId)
result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1)
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
# Get the Jacobians for the CoM of the end-effector link.
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
def reset_orientation(self, orientation): p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation)
import pybullet as p
import time
p.connect(p.DIRECT)
p.setGravity(0,0,-10)
p.setPhysicsEngineParameter(numSolverIterations=5)
p.setPhysicsEngineParameter(fixedTimeStep=1./240.)
p.setPhysicsEngineParameter(numSubSteps=1)
objects = p.loadMJCF("mjcf/humanoid_symmetric.xml")
ob = objects[0]
p.resetBasePositionAndOrientation(ob,[0.000000,0.000000,0.000000],[0.000000,0.000000,0.000000,1.000000])
ob = objects[1]
p.resetBasePositionAndOrientation(ob,[0.789351,0.962124,0.113124],[0.710965,0.218117,0.519402,-0.420923])
jointPositions=[ -0.200226, 0.123925, 0.000000, -0.224016, 0.000000, -0.022247, 0.099119, -0.041829, 0.000000, -0.344372, 0.000000, 0.000000, 0.090687, -0.578698, 0.044461, 0.000000, -0.185004, 0.000000, 0.000000, 0.039517, -0.131217, 0.000000, 0.083382, 0.000000, -0.165303, -0.140802, 0.000000, -0.007374, 0.000000 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
#first let the humanoid fall
#p.setRealTimeSimulation(1)
#time.sleep(5)
p.setRealTimeSimulation(0)
#p.saveWorld("lyiing.py")
#now do a benchmark
print("Starting benchmark")
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"pybullet_humanoid_timings.json")
for i in range(1000):
p.stepSimulation()
p.stopStateLogging(logId)
def reset_pose(self, position, orientation): p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], position, orientation)
print(d[0])
while True:
with open("../input_data/position4photos/positions.yml", "r") as f:
data = yaml.load(f, Loader=yaml.SafeLoader)
data = data[gym_interface.template(body)]
joint_positions = p.getJointStates(robot,
list(range(p.getNumJoints(robot))))
joint_positions = np.zeros(p.getNumJoints(robot))
if "position" in data:
robot_position = [0, 0, 0]
for key in data["position"]:
robot_position[key] = data["position"][key]
p.resetBasePositionAndOrientation(robot, robot_position,
[0, 0, 0, 1])
if "joints" in data:
for key in data["joints"]:
joint_positions[key] = data["joints"][key]
if "joints_name" in data:
for key in data["joints_name"]:
joint_positions[joint_names[key]] = data["joints_name"][key]
for joint_id in range(p.getNumJoints(robot)):
if joint_positions[joint_id] != 0:
p.resetJointState(robot,
joint_id,
targetValue=joint_positions[joint_id])
def set_config(robot, joint_id, link_id, base_pos, base_quat, joint_pos):
p.resetBasePositionAndOrientation(robot, base_pos, base_quat)
for k, v in joint_pos.items():
p.resetJointState(robot, joint_id[k], v, 0.)
os.sys.path.insert(0, currentdir)
import pybullet_data
from pkg_resources import parse_version
kuka_handId = 3
xpos = 0.43
ypos = 0.635
zpos = 0.909
ang = 3.14 * 0.5
orn = p.getQuaternionFromEuler([0, 0, ang])
p.connect(p.GUI)
finger = p.loadSDF("./model.sdf")
fingerID = finger[0]
print("fingerID:::", fingerID)
p.resetBasePositionAndOrientation(fingerID, [0.00000, 0.200000, 0.65000],
[0.000000, 0.000000, 0.000000, 1.000000])
table = p.loadURDF(
os.path.join(pybullet_data.getDataPath(), "table/table.urdf"), 0.5000000,
0.60000, 0.0000, 0.000000, 0.000000, 0.0, 1.0)
texUid = p.loadTexture("./cube_new/aaa.png")
cube_objects = p.loadSDF("./cube_new/model.sdf")
cubeId = cube_objects[0]
p.changeVisualShape(cube_objects[0], -1, rgbaColor=[1, 1, 1, 1])
p.changeVisualShape(cube_objects[0], -1, textureUniqueId=texUid)
p.resetBasePositionAndOrientation(cube_objects[0],
(0.5000000, 0.60000, 0.6700),
(0.717, 0.0, 0.0, 0.717))
#blockUid = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"block.urdf"), xpos,ypos,zpos,orn[0],orn[1],orn[2],orn[3])
blockUid = 2
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), [0, 0, 0])
def resetPose(self, pos, rot):
pb.resetBasePositionAndOrientation(self.object_id, pos, rot)
p.createMultiBody(0, baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeIds, baseOrientation=orn)
filename = os.path.expanduser("~/data/shuri-castle-shurijo-naha-okinawa-wip/source/Shujiro_Castle/Shujiro_Castle.obj")
collisionShapeId = p.createCollisionShape(p.GEOM_MESH, fileName=filename, flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
visualShapeIds = p.createVisualShape(p.GEOM_MESH, fileName=filename)
orn = p.getQuaternionFromEuler([0, 0, 0])
p.createMultiBody(0, baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeIds, baseOrientation=orn)
'''
print(p.getBodyInfo(collisionShapeId))
sphere = p.loadURDF("sphere_small.urdf")
#objects = p.loadMJCF("mjcf/sphere.xml")
#sphere = objects[0]
p.resetBasePositionAndOrientation(sphere, [0, 0, startHeight], [0, 0, 0, 1])
p.changeDynamics(sphere, -1, linearDamping=linearDamping)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
p.setGravity(0, 0, -10)
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
joystick = None
pygame.init()
if pygame.joystick.get_count():
0.170571, 0.9927545808, -0.02090099117, 0.08882396249, -0.07817796699,
-0.391532, 0.9828788495, 0.1013909845, -0.05515999155, 0.143618978,
0.9659421276, 0.1884590249, -0.1422460188, 0.105854014, 0.581348
]
startVel = [
1.235314324, -0.008525509087, 0.1515293946, -1.161516553, 0.1866449799,
-0.1050802848, 0, 0.935706195, 0.08277326387, 0.3002461862, 0, 0, 0, 0, 0,
1.114409628, 0.3618553952, -0.4505575061, 0, -1.725374735, -0.5052852598,
-0.8555179722, -0.2221173515, 0, -0.1837617357, 0.00171895706,
0.03912837591, 0, 0.147945294, 1.837653345, 0.1534535548, 1.491385941, 0,
-4.632454387, -0.9111172777, -1.300648184, -1.345694622, 0, -1.084238535,
0.1313680236, -0.7236998534, 0, -0.5278312973
]
p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1])
p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1])
index0 = 7
for j in range(p.getNumJoints(humanoid)):
ji = p.getJointInfo(humanoid, j)
targetPosition = [0]
jointType = ji[2]
if (jointType == p.JOINT_SPHERICAL):
targetPosition = [
startPose[index0 + 1], startPose[index0 + 2],
startPose[index0 + 3], startPose[index0 + 0]
]
targetVel = [
import pybullet as p
import pybullet_data
import time
import pybullet_data
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadSDF("stadium.sdf")
p.setGravity(0, 0, -10)
objects = p.loadMJCF("mjcf/sphere.xml")
sphere = objects[0]
p.resetBasePositionAndOrientation(sphere, [0, 0, 1], [0, 0, 0, 1])
p.changeDynamics(sphere, -1, linearDamping=0.9)
p.changeVisualShape(sphere, -1, rgbaColor=[1, 0, 0, 1])
forward = 0
turn = 0
forwardVec = [2, 0, 0]
cameraDistance = 1
cameraYaw = 35
cameraPitch = -35
while (1):
spherePos, orn = p.getBasePositionAndOrientation(sphere)
cameraTargetPosition = spherePos
p.resetDebugVisualizerCamera(cameraDistance, cameraYaw, cameraPitch,
cameraTargetPosition)
camInfo = p.getDebugVisualizerCamera()
]
objects = [
p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.800000, 0.000000,
0.000000, 0.000000, 1.000000)
]
objects = [
p.loadURDF("lego/lego.urdf", 1.000000, -0.200000, 0.900000, 0.000000,
0.000000, 0.000000, 1.000000)
]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0]
print("kuka gripper=")
print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper,
[0.923103, -0.200000, 1.250036],
[-0.000000, 0.964531, -0.000002, -0.263970])
jointPositions = [
0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055,
0.000000
]
for jointIndex in range(p.getNumJoints(kuka_gripper)):
p.resetJointState(kuka_gripper, jointIndex, jointPositions[jointIndex])
p.setJointMotorControl2(kuka_gripper, jointIndex, p.POSITION_CONTROL,
jointPositions[jointIndex], 0)
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper, 0, p.JOINT_FIXED,
[0, 0, 0], [0, 0, 0.05], [0, 0, 0])
objects = [
p.loadURDF("jenga/jenga.urdf", 1.300000, -0.700000, 0.750000, 0.000000,
def _set_fields_of_pose_of(self, pos, orn):
"""Set pose of body part"""
p.resetBasePositionAndOrientation(self.bodies[self.body_index], pos,
orn)
def reset(self):
self.setup_timing()
self.task_success = 0
self.human, self.bed, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(
furniture_type='bed',
static_human_base=False,
human_impairment='no_tremor',
print_joints=False,
gender='random')
self.robot_both_arm_joint_indices = self.robot_left_arm_joint_indices + self.robot_right_arm_joint_indices
self.robot_right_lower_limits = self.robot_lower_limits[
self.robot_right_arm_joint_indices]
self.robot_right_upper_limits = self.robot_upper_limits[
self.robot_right_arm_joint_indices]
self.robot_left_lower_limits = self.robot_lower_limits[
self.robot_left_arm_joint_indices]
self.robot_left_upper_limits = self.robot_upper_limits[
self.robot_left_arm_joint_indices]
self.robot_lower_limits = self.robot_lower_limits[
self.robot_both_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[
self.robot_both_arm_joint_indices]
self.reset_robot_joints()
friction = 5
p.changeDynamics(self.bed,
-1,
lateralFriction=friction,
spinningFriction=friction,
rollingFriction=friction,
physicsClientId=self.id)
# Setup human in the air and let them settle into a resting pose on the bed
joints_positions = [(3, np.deg2rad(30))]
controllable_joints = []
self.world_creation.setup_human_joints(self.human,
joints_positions,
controllable_joints,
use_static_joints=False,
human_reactive_force=None)
p.resetBasePositionAndOrientation(self.human, [-0.25, 0.2, 0.95],
p.getQuaternionFromEuler(
[-np.pi / 2.0, 0, 0],
physicsClientId=self.id),
physicsClientId=self.id)
p.setGravity(0, 0, -1, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id)
# Add small variation in human joint positions
for j in range(p.getNumJoints(self.human, physicsClientId=self.id)):
if p.getJointInfo(self.human, j,
physicsClientId=self.id)[2] != p.JOINT_FIXED:
p.resetJointState(self.human,
jointIndex=j,
targetValue=self.np_random.uniform(
-0.1, 0.1),
targetVelocity=0,
physicsClientId=self.id)
# Let the person settle on the bed
for _ in range(100):
p.stepSimulation(physicsClientId=self.id)
friction = 0.3
p.changeDynamics(self.bed,
-1,
lateralFriction=friction,
spinningFriction=friction,
rollingFriction=friction,
physicsClientId=self.id)
# Lock human joints (except arm) and set velocities to 0
joints_positions = [(3, np.deg2rad(60)), (4, np.deg2rad(-60)), (6, 0)]
self.human_controllable_joint_indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices,
use_static_joints=True,
human_reactive_force=None)
p.changeDynamics(self.human, -1, mass=0, physicsClientId=self.id)
p.resetBaseVelocity(self.human,
linearVelocity=[0, 0, 0],
angularVelocity=[0, 0, 0],
physicsClientId=self.id)
for i in self.human_controllable_joint_indices:
p.resetJointState(self.human,
i,
targetValue=p.getJointState(
self.human, i, physicsClientId=self.id)[0],
targetVelocity=0,
physicsClientId=self.id)
for _ in range(100):
p.stepSimulation(physicsClientId=self.id)
human_joint_states = p.getJointStates(
self.human,
jointIndices=self.human_controllable_joint_indices,
physicsClientId=self.id)
self.target_human_joint_positions = np.array(
[x[0] for x in human_joint_states])
self.human_lower_limits = self.human_lower_limits[
self.human_controllable_joint_indices]
self.human_upper_limits = self.human_upper_limits[
self.human_controllable_joint_indices]
shoulder_pos, shoulder_orient = p.getLinkState(
self.human,
5,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human,
7,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human,
9,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
waist_pos = p.getLinkState(self.human,
24,
computeForwardKinematics=True,
physicsClientId=self.id)[0]
hips_pos = p.getLinkState(self.human,
27,
computeForwardKinematics=True,
physicsClientId=self.id)[0]
if self.robot_type == 'jaco':
bed_pos, bed_orient = p.getBasePositionAndOrientation(
self.bed, physicsClientId=self.id)
p.resetBasePositionAndOrientation(
self.robot,
np.array(bed_pos) + np.array([-0.7, 0.75, 0.6]),
p.getQuaternionFromEuler([0, 0, -np.pi / 2.0],
physicsClientId=self.id),
physicsClientId=self.id)
elif self.robot_type == 'kinova_gen3':
bed_pos, bed_orient = p.getBasePositionAndOrientation(
self.bed, physicsClientId=self.id)
p.resetBasePositionAndOrientation(
self.robot,
np.array(bed_pos) + np.array([-0.7, 0.75, 0.6]),
p.getQuaternionFromEuler([0, 0, -np.pi / 2.0],
physicsClientId=self.id),
physicsClientId=self.id)
target_pos_right = np.array(
[-0.9, -0.3, 0.8]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_pos_left = np.array([-0.9, 0.7, 0.8]) + self.np_random.uniform(
-0.05, 0.05, size=3)
if self.robot_type == 'pr2':
target_orient = np.array(
p.getQuaternionFromEuler(np.array([0, 0, 0]),
physicsClientId=self.id))
self.position_robot_toc(
self.robot, [54, 77], [[(target_pos_right, target_orient)],
[(target_pos_left, target_orient)]],
[[(wrist_pos, None),
(hips_pos, None)], [(elbow_pos, None), (waist_pos, None)]], [
self.robot_right_arm_joint_indices,
self.robot_left_arm_joint_indices
],
[self.robot_right_lower_limits, self.robot_left_lower_limits],
[self.robot_right_upper_limits, self.robot_left_upper_limits],
ik_indices=[range(15, 15 + 7),
range(29, 29 + 7)],
pos_offset=np.array([-0.3, 0.7, 0]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.world_creation.set_gripper_open_position(
self.robot,
position=0.15,
left=True,
set_instantly=True,
indices=[81, 82, 83, 84])
self.world_creation.set_gripper_open_position(
self.robot,
position=0.15,
left=False,
set_instantly=True,
indices=[58, 59, 60, 61])
elif self.robot_type in ['jaco', 'kinova_gen3']:
if self.robot_type == 'jaco':
target_pos_left = np.array([
-0.9, 0.4, 1
]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_orient = p.getQuaternionFromEuler(
np.array([0, np.pi / 2.0, 0]), physicsClientId=self.id)
base_position, base_orientation, _ = self.position_robot_toc(
self.robot,
8, [(target_pos_left, target_orient)], [(wrist_pos, None),
(hips_pos, None),
(elbow_pos, None),
(waist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_left_lower_limits,
self.robot_left_upper_limits,
ik_indices=[0, 1, 2, 3, 4, 5, 6],
pos_offset=np.array([-0.05, 1.15, 0.6]),
max_ik_iterations=200,
step_sim=True,
random_position=0.1,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.world_creation.set_gripper_open_position(
self.robot,
position=1.05,
left=False,
set_instantly=True,
indices=[10, 12, 14])
else:
base_position = np.array([-0.05, 0, 0])
target_pos_left = np.array([
-0.9, 0.5, 1
]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_orient = p.getQuaternionFromEuler(
np.array([0, np.pi / 2.0, 0]), physicsClientId=self.id)
self.util.ik_random_restarts(self.robot,
7,
target_pos_left,
target_orient,
self.world_creation,
self.robot_left_arm_joint_indices,
self.robot_left_lower_limits,
self.robot_left_upper_limits,
ik_indices=[0, 1, 2, 3, 4, 5, 6],
max_iterations=100,
max_ik_random_restarts=40,
random_restart_threshold=0.03,
step_sim=True)
# Load a nightstand in the environment for the jaco arm
self.nightstand_scale = 0.275
visual_filename = os.path.join(self.world_creation.directory,
'nightstand', 'nightstand.obj')
collision_filename = os.path.join(self.world_creation.directory,
'nightstand', 'nightstand.obj')
nightstand_visual = p.createVisualShape(
shapeType=p.GEOM_MESH,
fileName=visual_filename,
meshScale=[self.nightstand_scale] * 3,
rgbaColor=[0.5, 0.5, 0.5, 1.0],
physicsClientId=self.id)
nightstand_collision = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=collision_filename,
meshScale=[self.nightstand_scale] * 3,
physicsClientId=self.id)
nightstand_pos = np.array([-0.9, 0.8, 0]) + base_position
nightstand_orient = p.getQuaternionFromEuler(
np.array([np.pi / 2.0, 0, 0]), physicsClientId=self.id)
self.nightstand = p.createMultiBody(
baseMass=0,
baseCollisionShapeIndex=nightstand_collision,
baseVisualShapeIndex=nightstand_visual,
basePosition=nightstand_pos,
baseOrientation=nightstand_orient,
baseInertialFramePosition=[0, 0, 0],
useMaximalCoordinates=False,
physicsClientId=self.id)
else:
target_orient = p.getQuaternionFromEuler(np.array(
[0, -np.pi / 2.0, np.pi]),
physicsClientId=self.id)
if self.robot_type == 'baxter':
self.position_robot_toc(
self.robot, [26, 49], [[(target_pos_right, target_orient)],
[(target_pos_left, target_orient)]],
[[(wrist_pos, None),
(hips_pos, None)], [(elbow_pos, None),
(waist_pos, None)]],
[
self.robot_right_arm_joint_indices,
self.robot_left_arm_joint_indices
], [
self.robot_right_lower_limits,
self.robot_left_lower_limits
], [
self.robot_right_upper_limits,
self.robot_left_upper_limits
],
ik_indices=[range(1, 8), range(10, 17)],
pos_offset=np.array([-0.3, 0.6, 0.975]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.world_creation.set_gripper_open_position(
self.robot,
position=0.01,
left=True,
set_instantly=True,
indices=[50, 52])
self.world_creation.set_gripper_open_position(
self.robot,
position=0.01,
left=False,
set_instantly=True,
indices=[27, 29])
else:
self.position_robot_toc(
self.robot,
19, [(target_pos_left, target_orient)],
[(wrist_pos, None), (hips_pos, None), (elbow_pos, None),
(waist_pos, None)],
self.robot_left_arm_joint_indices,
self.robot_left_lower_limits,
self.robot_left_upper_limits,
ik_indices=[0, 2, 3, 4, 5, 6, 7],
pos_offset=np.array([-0.3, 0.6, 0.975]),
max_ik_iterations=200,
step_sim=True,
check_env_collisions=False,
human_joint_indices=self.human_controllable_joint_indices,
human_joint_positions=self.target_human_joint_positions)
self.world_creation.set_gripper_open_position(
self.robot, position=0.01, left=True, set_instantly=True)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
return self._get_obs([0, 0], [0, 0, 0])
def step(self, action):
done = False
#reward (float): amount of reward achieved by the previous action. The scale varies between environments, but the goal is always to increase your total reward.
#done (boolean): whether it's time to reset the environment again. Most (but not all) tasks are divided up into well-defined episodes, and done being True indicates the episode has terminated. (For example, perhaps the pole tipped too far, or you lost your last life.)
#observation (object): an environment-specific object representing your observation of the environment. For example, pixel data from a camera, joint angles and joint velocities of a robot, or the board state in a board game.
#info (dict): diagnostic information useful for debugging. It can sometimes be useful for learning (for example, it might contain the raw probabilities behind the environment's last state change). However, official evaluations of your agent are not allowed to use this for learning.
def convertSensor(finger_index):
if finger_index == self.indexId:
return random.uniform(-1, 1)
#return 0
else:
#return random.uniform(-1,1)
return 0
def convertAction(action):
#converted = (action-30)/10
#converted = (action-16)/10
if action == 6:
converted = -1
elif action == 25:
converted = 1
#print("action ",action)
#print("converted ",converted)
return converted
aspect = 1
camTargetPos = [0, 0, 0]
yaw = 40
pitch = 10.0
roll = 0
upAxisIndex = 2
camDistance = 4
pixelWidth = 320
pixelHeight = 240
nearPlane = 0.0001
farPlane = 0.022
lightDirection = [0, 1, 0]
lightColor = [1, 1, 1] #optional
fov = 50 #10 or 50
hand_po = pb.getBasePositionAndOrientation(self.agent)
#print("action",action)
ho = pb.getQuaternionFromEuler([0, 0, 0])
#hand_cid = pb.createConstraint(self.hand,-1,-1,-1,pb.JOINT_FIXED,[0,0,0],(0.1,0,0),hand_po[0],ho,hand_po[1])
if action == 65298 or action == 0: #down
#pb.changeConstraint(hand_cid,(hand_po[0][0]+self.move,hand_po[0][1],hand_po[0][2]),hand_po[1], maxForce=50)
pb.resetBasePositionAndOrientation(
self.agent,
(hand_po[0][0] + self.move, hand_po[0][1], hand_po[0][2]),
hand_po[1])
elif action == 65297 or action == 1: #up
pb.resetBasePositionAndOrientation(
self.agent,
(hand_po[0][0] - self.move, hand_po[0][1], hand_po[0][2]),
hand_po[1])
#pb.changeConstraint(hand_cid,(hand_po[0][0]-self.move,hand_po[0][1],hand_po[0][2]),hand_po[1], maxForce=50)
elif action == 65295 or action == 2: #left
pb.resetBasePositionAndOrientation(
self.agent,
(hand_po[0][0], hand_po[0][1] + self.move, hand_po[0][2]),
hand_po[1])
#pb.changeConstraint(hand_cid,(hand_po[0][0],hand_po[0][1]+self.move,hand_po[0][2]),hand_po[1], maxForce=50)
elif action == 65296 or action == 3: #right
pb.resetBasePositionAndOrientation(
self.agent,
(hand_po[0][0], hand_po[0][1] - self.move, hand_po[0][2]),
hand_po[1])
#pb.changeConstraint(hand_cid,(hand_po[0][0],hand_po[0][1]-self.move,hand_po[0][2]),hand_po[1], maxForce=50)
elif action == 44 or action == 4: #<
pb.resetBasePositionAndOrientation(
self.agent,
(hand_po[0][0], hand_po[0][1], hand_po[0][2] + self.move),
hand_po[1])
#pb.changeConstraint(hand_cid,(hand_po[0][0],hand_po[0][1],hand_po[0][2]+self.move),hand_po[1], maxForce=50)
elif action == 46 or action == 5: #>
pb.resetBasePositionAndOrientation(
self.agent,
(hand_po[0][0], hand_po[0][1], hand_po[0][2] - self.move),
hand_po[1])
#pb.changeConstraint(hand_cid,(hand_po[0][0],hand_po[0][1],hand_po[0][2]-self.move),hand_po[1], maxForce=50)
elif action >= 6 and action <= 7:
#elif action >= 6 and action <= 40:
if action == 7:
action = 25 #bad kludge redo all this code
pink = convertSensor(self.pinkId)
middle = convertSensor(self.middleId)
index = convertAction(action)
thumb = convertSensor(self.thumbId)
ring = convertSensor(self.ring_id)
pb.setJointMotorControl2(self.agent, 7, pb.POSITION_CONTROL,
self.pi / 4.)
pb.setJointMotorControl2(self.agent, 9, pb.POSITION_CONTROL,
thumb + self.pi / 10)
pb.setJointMotorControl2(self.agent, 11, pb.POSITION_CONTROL,
thumb)
pb.setJointMotorControl2(self.agent, 13, pb.POSITION_CONTROL,
thumb)
# That's index finger parts
pb.setJointMotorControl2(self.agent, 17, pb.POSITION_CONTROL,
index)
pb.setJointMotorControl2(self.agent, 19, pb.POSITION_CONTROL,
index)
pb.setJointMotorControl2(self.agent, 21, pb.POSITION_CONTROL,
index)
pb.setJointMotorControl2(self.agent, 24, pb.POSITION_CONTROL,
middle)
pb.setJointMotorControl2(self.agent, 26, pb.POSITION_CONTROL,
middle)
pb.setJointMotorControl2(self.agent, 28, pb.POSITION_CONTROL,
middle)
pb.setJointMotorControl2(self.agent, 40, pb.POSITION_CONTROL, pink)
pb.setJointMotorControl2(self.agent, 42, pb.POSITION_CONTROL, pink)
pb.setJointMotorControl2(self.agent, 44, pb.POSITION_CONTROL, pink)
ringpos = 0.5 * (pink + middle)
pb.setJointMotorControl2(self.agent, 32, pb.POSITION_CONTROL,
ringpos)
pb.setJointMotorControl2(self.agent, 34, pb.POSITION_CONTROL,
ringpos)
pb.setJointMotorControl2(self.agent, 36, pb.POSITION_CONTROL,
ringpos)
if self.downCameraOn: viewMatrix = down_view()
else: viewMatrix = self.ahead_view()
projectionMatrix = pb.computeProjectionMatrixFOV(
fov, aspect, nearPlane, farPlane)
w, h, img_arr, depths, mask = pb.getCameraImage(
200,
200,
viewMatrix,
projectionMatrix,
lightDirection,
lightColor,
renderer=pb.ER_TINY_RENDERER)
#w,h,img_arr,depths,mask = pb.getCameraImage(200,200, viewMatrix,projectionMatrix, lightDirection,lightColor,renderer=pb.ER_BULLET_HARDWARE_OPENGL)
#red_dimension = img_arr[:,:,0] #TODO change this so any RGB value returns 1, anything else is 0
red_dimension = img_arr[:, :, 0].flatten(
) #TODO change this so any RGB value returns 1, anything else is 0
#observation = red_dimension
self.img_arr = img_arr
observation = (np.absolute(red_dimension - 255) > 0).astype(int)
self.current_observation = observation
self.img_arr = img_arr
self.depths = depths
info = [42] #answer to life,TODO use real values
pb.stepSimulation()
self.steps += 1
#reward if moving towards the object or touching the object
reward = 0
max_steps = 1000
if self.is_touching():
touch_reward = 10
if 'debug' in self.options and self.options['debug'] == True:
print("TOUCHING!!!!")
else:
touch_reward = 0
obj_po = pb.getBasePositionAndOrientation(self.obj_to_classify)
distance = math.sqrt(
sum([(xi - yi)**2 for xi, yi in zip(obj_po[0], hand_po[0])
])) #TODO faster euclidean
#distance = np.linalg.norm(obj_po[0],hand_po[0])
#print("distance:",distance)
if distance < self.prev_distance:
reward += 1 * (max_steps - self.steps)
elif distance > self.prev_distance:
reward -= 10
reward -= distance
reward += touch_reward
self.prev_distance = distance
#print("shape",observation.shape)
if 'debug' in self.options and self.options['debug'] == True:
print("touch reward ", touch_reward)
print("action ", action)
print("reward ", reward)
print("distance ", distance)
if self.steps >= max_steps or self.is_touching():
done = True
return observation, reward, done, info
def set_pose(body, pose):
(point, quat) = pose
p.resetBasePositionAndOrientation(body,
point,
quat,
physicsClientId=CLIENT)
def set_orientation(self, orn):
old_pos, _ = p.getBasePositionAndOrientation(self.body_id)
p.resetBasePositionAndOrientation(self.body_id, old_pos, orn)
import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
p.setGravity(0,0,-10)
arm = p.loadURDF("widowx/widowx.urdf",useFixedBase=True)
p.resetBasePositionAndOrientation(arm,[-0.098612,-0.000726,-0.194018],[0.000000,0.000000,0.000000,1.000000])
while (1):
p.stepSimulation()
time.sleep(0.01)
#p.saveWorld("test.py")
viewMat = p.getDebugVisualizerCamera()[2]
#projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
projMatrix = p.getDebugVisualizerCamera()[3]
width=640
height=480
img_arr = p.getCameraImage(width=width,height=height,viewMatrix=viewMat,projectionMatrix=projMatrix)
def reset_orientation(self, orientation): p.resetBasePositionAndOrientation(self.bodies[self.bodyIndex], self.get_position(), orientation)
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True)
rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)
while True:
roll = p.readUserDebugParameter(rollId)
pitch = p.readUserDebugParameter(pitchId)
yaw = p.readUserDebugParameter(yawId)
x = p.readUserDebugParameter(fwdxId)
y = p.readUserDebugParameter(fwdyId)
z = p.readUserDebugParameter(fwdzId)
orn = p.getQuaternionFromEuler([roll, pitch, yaw])
p.resetBasePositionAndOrientation(q, [x, y, z], orn)
#p.stepSimulation()#not really necessary for this demo, no physics used
lineWidth=lineWidth,
lifeTime=0)
pitch = 0
yaw = 0
while (p.isConnected()):
pitch += 0.01
if (pitch >= 3.1415 * 2.):
pitch = 0
yaw += 0.01
if (yaw >= 3.1415 * 2.):
yaw = 0
baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0])
if (obB >= 0):
p.resetBasePositionAndOrientation(obB, basePositionB, baseOrientationB)
if (useCollisionShapeQuery):
pts = p.getClosestPoints(bodyA=-1,
bodyB=-1,
distance=100,
collisionShapeA=geom,
collisionShapeB=geomBox,
collisionShapePositionA=[0.5, 0, 1],
collisionShapePositionB=basePositionB,
collisionShapeOrientationB=baseOrientationB)
#pts = p.getClosestPoints(bodyA=obA, bodyB=-1, distance=100, collisionShapeB=geomBox, collisionShapePositionB=basePositionB, collisionShapeOrientationB=baseOrientationB)
else:
pts = p.getClosestPoints(bodyA=obA, bodyB=obB, distance=100)
if len(pts) > 0:
import pybullet as p
import time
import pybullet_data
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True)
rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)
while True:
roll = p.readUserDebugParameter(rollId)
pitch = p.readUserDebugParameter(pitchId)
yaw = p.readUserDebugParameter(yawId)
x = p.readUserDebugParameter(fwdxId)
y = p.readUserDebugParameter(fwdyId)
z = p.readUserDebugParameter(fwdzId)
orn = p.getQuaternionFromEuler([roll, pitch, yaw])
p.resetBasePositionAndOrientation(q, [x, y, z], orn)
#p.stepSimulation()#not really necessary for this demo, no physics used
def add_ball(self, x_des, y_des):
self.ball1 = p.loadURDF("urdf/ball1.urdf")
p.resetBasePositionAndOrientation(self.ball1, [x_des-0.3, -0.05, 0.285+y_des], (0., 0., 0.5, 0.5))
return log
#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
p.loadURDF("cube.urdf",[2,2,5])
p.loadURDF("cube.urdf",[-2,-2,5])
p.loadURDF("cube.urdf",[2,-2,5])
log = readLogFile("LOG0001.txt")
recordNum = len(log)
itemNum = len(log[0])
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)
for record in log:
Id = record[2]
pos = [record[3],record[4],record[5]]
orn = [record[6],record[7],record[8],record[9]]
p.resetBasePositionAndOrientation(Id,pos,orn)
numJoints = p.getNumJoints(Id)
for i in range (numJoints):
jointInfo = p.getJointInfo(Id,i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(Id,i,record[qIndex-7+17])
sleep(0.0005)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0]
print ("kuka gripper=")
print(kuka_gripper)
p.resetBasePositionAndOrientation(kuka_gripper,[0.923103,-0.200000,1.250036],[-0.000000,0.964531,-0.000002,-0.263970])
jointPositions=[ 0.000000, -0.011130, -0.206421, 0.205143, -0.009999, 0.000000, -0.010055, 0.000000 ]
for jointIndex in range (p.getNumJoints(kuka_gripper)):
p.resetJointState(kuka_gripper,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka_gripper,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0])
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
def update_velocities(self, robot_data, velocities_dict):
"""Updates a given velocity command."""
pose = robot_data.pose()
lin_vel = robot_data.linear_velocity()
ang_vel = robot_data.angular_velocity()
inv_pos, inv_rot = pb.invertTransform(pose.position, pose.quaternion)
ZERO_VEC = (0, 0, 0)
ZERO_ROT = (0, 0, 0, 1)
if self.z_ang_joint in velocities_dict:
c_ang_ib, _ = pb.multiplyTransforms(inv_pos, inv_rot, ang_vel,
ZERO_ROT)
d_ang_vel = max(
min(velocities_dict[self.z_ang_joint], self.m_ang_v),
-self.m_ang_v)
ang_gain = max(min(d_ang_vel - c_ang_ib[2], self.m_ang_gain),
-self.m_ang_gain)
del velocities_dict[self.z_ang_joint]
ang_vel, _ = pb.multiplyTransforms(
ZERO_VEC, pose.quaternion, [0.0, 0.0, c_ang_ib[2] + ang_gain],
ZERO_ROT)
else:
ang_vel = (0, 0, 0)
fwd_dir, _ = pb.multiplyTransforms(ZERO_VEC, pose.quaternion,
[1, 0, 0], ZERO_ROT)
yaw = atan2(fwd_dir[1], fwd_dir[0])
if self.x_lin_joint in velocities_dict or self.y_lin_joint in velocities_dict:
d_x_vel = 0
d_y_vel = 0
if self.x_lin_joint in velocities_dict:
d_x_vel = max(
min(velocities_dict[self.x_lin_joint], self.m_lin_v),
-self.m_lin_v)
del velocities_dict[self.x_lin_joint]
if self.y_lin_joint in velocities_dict:
d_y_vel = max(
min(velocities_dict[self.y_lin_joint], self.m_lin_v),
-self.m_lin_v)
del velocities_dict[self.y_lin_joint]
x_vel_gain = max(min(d_x_vel - lin_vel[0], self.m_vel_gain),
-self.m_vel_gain)
y_vel_gain = max(min(d_y_vel - lin_vel[1], self.m_vel_gain),
-self.m_vel_gain)
lin_vel = [lin_vel[0] + x_vel_gain, lin_vel[1] + y_vel_gain,
0] # lin_vel[2]]
else:
lin_vel = (0, 0, 0) #lin_vel[2])
new_quat = pb.getQuaternionFromEuler([0, 0, yaw])
#print(' '.join(['{}'.format(type(c)) for c in list(lin_vel) + list(ang_vel)]))
# print('New position: {}\nNew velocity: {}'.format((pose.position[0], pose.position[1], robot_data.initial_pos[2]), lin_vel))
pb.resetBasePositionAndOrientation(
robot_data.bId(),
(pose.position[0], pose.position[1], robot_data.initial_pos[2]),
new_quat,
physicsClientId=robot_data.simulator.client_id())
pb.resetBaseVelocity(robot_data.bId(),
lin_vel,
ang_vel,
physicsClientId=robot_data.simulator.client_id())
