import pybullet as p
import time
p.connect(p.GUI)
planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False)
cubeId = p.loadURDF("cube_collisionfilter.urdf", [0, 0, 3], useMaximalCoordinates=False)
collisionFilterGroup = 0
collisionFilterMask = 0
p.setCollisionFilterGroupMask(cubeId, -1, collisionFilterGroup, collisionFilterMask)
enableCollision = 1
p.setCollisionFilterPair(planeId, cubeId, -1, -1, enableCollision)
p.setRealTimeSimulation(1)
p.setGravity(0, 0, -10)
while (p.isConnected()):
time.sleep(1. / 240.)
p.setGravity(0, 0, -10)
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=",
enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, l0, l1,
enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, -1, -1, 1, -1, -1, -1, -1, -1, -1, -1, -1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
jointScale = np.pi / 180
W_DOF = 8
# fix joints [0,3,6,9] : no hip movement
# try changeConstraint
# if W_DOF == 8:
# joints[0:3:9] = 0
dir_flag = 1
import pybullet as p
import time
p.connect(p.GUI)
planeId = p.loadURDF("plane.urdf", useMaximalCoordinates=False)
cubeId = p.loadURDF("cube_collisionfilter.urdf", [0, 0, 3],
useMaximalCoordinates=False)
collisionFilterGroup = 0
collisionFilterMask = 0
p.setCollisionFilterGroupMask(cubeId, -1, collisionFilterGroup,
collisionFilterMask)
enableCollision = 1
p.setCollisionFilterPair(planeId, cubeId, -1, -1, enableCollision)
p.setRealTimeSimulation(1)
p.setGravity(0, 0, -10)
while (p.isConnected()):
time.sleep(1. / 240.)
p.setGravity(0, 0, -10)
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class, loads the robot into
the simulated instance. This method also updates the max velocity of
the robot's fingers, adds self collision filters to the model and
defines the cameras of the model in the camera_dict.
Parameters:
translation - List containing 3 elements, the translation [x, y, z]
of the robot in the WORLD frame
quaternion - List containing 4 elements, the quaternion
[x, y, z, q] of the robot in the WORLD frame
physicsClientId - The id of the simulated instance in which the
robot is supposed to be loaded
Returns:
boolean - True if the method ran correctly, False otherwise
"""
pybullet.setAdditionalSearchPath(os.path.dirname(
os.path.realpath(__file__)),
physicsClientId=physicsClientId)
# Add 0.50 meters on the z component, avoing to spawn NAO in the ground
translation = [translation[0], translation[1], translation[2] + 1.05]
RobotVirtual.loadRobot(self,
translation,
quaternion,
physicsClientId=physicsClientId)
balance_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=translation,
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.goToPosture("Stand", 1.0)
pybullet.setCollisionFilterPair(self.robot_model,
self.robot_model,
self.link_dict["REye"].getIndex(),
self.link_dict["LEye"].getIndex(),
0,
physicsClientId=self.physics_client)
for link in ["torso", "HeadRoll_link"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["NeckPitch_link"].getIndex(),
self.link_dict[link].getIndex(),
0,
physicsClientId=self.physics_client)
for side in ["R", "L"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Eye"].getIndex(),
self.link_dict["HeadRoll_link"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Thigh"].getIndex(),
self.link_dict["body"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side.lower() + "_ankle"].getIndex(),
self.link_dict[side + "Tibia"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "ShoulderYaw_link"].getIndex(),
self.link_dict["torso"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "WristRoll_link"].getIndex(),
self.link_dict[side.lower() + "_wrist"].getIndex(),
0,
physicsClientId=self.physics_client)
for link in ["ShoulderYaw_link", "WristYaw_link"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + link].getIndex(),
self.link_dict[side + "Elbow"].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
if side + "finger" in name.lower() or\
side + "thumb" in name.lower():
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side.lower + "_wrist"].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
for joint in self.joint_dict.values():
pybullet.resetJointState(self.robot_model, joint.getIndex(), 0.0)
pybullet.removeConstraint(balance_constraint,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
camera_right = CameraRgb(
self.robot_model,
RomeoVirtual.ID_CAMERA_RIGHT,
self.link_dict["CameraRightEye_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
camera_left = CameraRgb(self.robot_model,
RomeoVirtual.ID_CAMERA_LEFT,
self.link_dict["CameraLeftEye_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
camera_depth = CameraDepth(self.robot_model,
RomeoVirtual.ID_CAMERA_DEPTH,
self.link_dict["CameraDepth_optical_frame"],
hfov=58.0,
vfov=45.0,
physicsClientId=self.physics_client)
self.camera_dict = {
RomeoVirtual.ID_CAMERA_RIGHT: camera_right,
RomeoVirtual.ID_CAMERA_LEFT: camera_left,
RomeoVirtual.ID_CAMERA_DEPTH: camera_depth
}
def load(self):
ids = super(JR2_Kinova_Head, self).load()
robot_id = self.robot_ids[0]
# disable collision between base_chassis_joint and pan_joint
# between base_chassis_joint and tilt_joint
# between base_chassis_joint and camera_joint
# between jr2_fixed_body_joint and pan_joint
# between jr2_fixed_body_joint and tilt_joint
# between jr2_fixed_body_joint and camera_joint
p.setCollisionFilterPair(robot_id, robot_id, 0, 31, 0)
p.setCollisionFilterPair(robot_id, robot_id, 0, 32, 0)
p.setCollisionFilterPair(robot_id, robot_id, 0, 33, 0)
p.setCollisionFilterPair(robot_id, robot_id, 1, 31, 0)
p.setCollisionFilterPair(robot_id, robot_id, 1, 32, 0)
p.setCollisionFilterPair(robot_id, robot_id, 1, 33, 0)
return ids
def ignoreCollisions(botID1, botID2):
nJoints1 = p.getNumJoints(botID1)
nJoints2 = p.getNumJoints(botID2)
for i in range(-1, nJoints1):
for j in range(-1, nJoints2):
p.setCollisionFilterPair(botID1, botID2, i, j, 0)
def reset(self, ret_images=False, ret_dict=False):
self.setup_timing()
self.task_success = 0
self.human, self.wheelchair, 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='wheelchair',
static_human_base=True,
human_impairment='random',
print_joints=False,
gender='random')
self.robot_lower_limits = self.robot_lower_limits[
self.robot_right_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[
self.robot_right_arm_joint_indices]
self.reset_robot_joints()
#
# if self.robot_type == 'jaco':
# wheelchair_pos, wheelchair_orient = p.getBasePositionAndOrientation(self.wheelchair,
# physicsClientId=self.id)
# p.resetBasePositionAndOrientation(self.robot, np.array(wheelchair_pos) + np.array([-0.35, -0.3, 0.3]),
# p.getQuaternionFromEuler([0, 0, -np.pi / 2.0], physicsClientId=self.id),
# physicsClientId=self.id)
# base_pos, base_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id)
joints_positions = [(6, np.deg2rad(-90)), (16, np.deg2rad(-90)),
(28, np.deg2rad(-90)), (31, np.deg2rad(80)),
(35, np.deg2rad(-90)), (38, np.deg2rad(80))]
joints_positions += [
(21, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))),
(22, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))),
(23, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30)))
]
self.human_controllable_joint_indices = [20, 21, 22, 23]
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices if
(self.human_control
or self.world_creation.human_impairment == 'tremor') else [],
use_static_joints=True,
human_reactive_force=None)
p.resetBasePositionAndOrientation(
self.human, [0, 0.03, 0.89 if self.gender == 'male' else 0.86],
[0, 0, 0, 1],
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]
# Place a bowl of food on a table
self.table = p.loadURDF(os.path.join(self.world_creation.directory,
'table', 'table_tall.urdf'),
basePosition=[0.35, -0.9, 0],
baseOrientation=p.getQuaternionFromEuler(
[0, 0, 0], physicsClientId=self.id),
physicsClientId=self.id)
# MOUTH SIM
self.mouth_pos = [-0, -0.15, 1.2]
self.mouth_orient = p.getQuaternionFromEuler(
[np.pi / 2, np.pi / 2, -np.pi / 2], physicsClientId=self.id)
self.mouth = p.loadURDF(os.path.join(self.world_creation.directory,
'mouth', 'hole.urdf'),
useFixedBase=True,
basePosition=self.mouth_pos,
baseOrientation=self.mouth_orient,
flags=p.URDF_USE_SELF_COLLISION,
physicsClientId=self.id)
sph_vis = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.005,
rgbaColor=[0, 1, 0, 1],
physicsClientId=self.id)
self.mouth_center_vis = p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sph_vis,
basePosition=self.mouth_pos,
useMaximalCoordinates=False,
physicsClientId=self.id)
self.batch_ray_offsets = [[0.08, 0, 0], [-0.08, 0, 0], [0, 0, 0.05],
[0, 0, -0.05]]
self.ray_markers = [
p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sph_vis,
basePosition=self.mouth_pos,
useMaximalCoordinates=False,
physicsClientId=self.id) for i in range(4)
]
self.ray_markers_end = [
p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sph_vis,
basePosition=self.mouth_pos,
useMaximalCoordinates=False,
physicsClientId=self.id) for i in range(4)
]
# SCALE = 0.1
# self.mouthVisualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
# fileName=os.path.join(self.world_creation.directory, 'mouth', 'hole2.obj'),
# rgbaColor=[0.8, 0.4, 0, 1],
# # specularColor=[0.4, .4, 0],
# visualFramePosition=[0, 0, 0],
# meshScale=[SCALE] * 3,
# physicsClientId=self.id)
# self.mouthCollisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
# fileName=os.path.join(self.world_creation.directory, 'mouth', 'hole2.obj'),
# collisionFramePosition=[0, 0, 0],
# meshScale=[SCALE] * 3,
# flags=p.GEOM_FORCE_CONCAVE_TRIMESH,
# physicsClientId=self.id)
# self.mouth = p.createMultiBody(baseMass=0.1,
# baseInertialFramePosition=[0, 0, 0],
# baseCollisionShapeIndex=self.mouthCollisionShapeId,
# baseVisualShapeIndex=self.mouthVisualShapeId,
# basePosition=[0, 0, 0],
# useMaximalCoordinates=True,
# flags=p.URDF_USE_SELF_COLLISION,
# physicsClientId=self.id)
p.resetDebugVisualizerCamera(cameraDistance=1.10,
cameraYaw=40,
cameraPitch=-45,
cameraTargetPosition=[-0.2, 0, 0.75],
physicsClientId=self.id)
# ROBOT STUFF
# target_pos = np.array(bowl_pos) + np.array([0, -0.1, 0.4]) + self.np_random.uniform(-0.05, 0.05, size=3)
if self.robot_type == 'panda':
# target_pos = [0.2, -0.7, 1.4]
# target_orient = [-0.7350878727462702, -8.032928869253401e-09, 7.4087721728719465e-09,
# 0.6779718425874067]
# target_orient = [0.4902888273008801, -0.46462044731135954, -0.5293302738768326, -0.5133752690981496]
# target_orient = p.getQuaternionFromEuler(np.array([0, 0, np.pi / 2.0]), physicsClientId=self.id)
# self.util.ik_random_restarts(self.robot, 8, target_pos, target_orient, self.world_creation,
# self.robot_right_arm_joint_indices, self.robot_lower_limits,
# self.robot_upper_limits,
# ik_indices=range(len(self.robot_right_arm_joint_indices)), max_iterations=1000,
# max_ik_random_restarts=40, random_restart_threshold=0.01, step_sim=True,
# check_env_collisions=True)
#
# positions = [0.42092164, -0.92326318, -0.33538581, -2.65185322, 1.40763901, 1.81818155, 0.58610855, 0, 0,
# 0.02, 0.02] # 11
# positions = [0.4721324, -0.87257245, -0.2123101, -2.44757844, 1.47352227, 1.90545005, 0.75139663]
positions = [
0.44594466, -0.58616227, -0.78082232, -2.59866731, 1.15649738,
1.54176148, 0.15145287
]
for idx in range(len(positions)):
p.resetJointState(self.robot, idx, positions[idx])
self.world_creation.set_gripper_open_position(self.robot,
position=0.00,
left=False,
set_instantly=True)
self.drop_fork = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.08] * 3,
pos_offset=[0, 0, 0.08], # fork: [0, -0.02, 0.16],
orient_offset=p.getQuaternionFromEuler(
[-0, -np.pi, 0], physicsClientId=self.id),
left=False,
maximal=False)
else:
raise NotImplementedError
# LOAD FOOD ITEM
self.food_type = np.random.randint(0, len(self.foods), dtype=int)
self.food_scale = np.random.uniform(
self.foods_scale_range[self.food_type][0],
self.foods_scale_range[self.food_type][1])
print("LOADING food file: %s with scale %.2f" %
(self.foods[self.food_type], self.food_scale))
self.child_offset = self.food_scale * self.food_offsets[
self.food_type] # [-0.004, -0.0065]
self.food_orient_eul = np.random.rand(3) * 2 * np.pi
self.food_orient_quat = p.getQuaternionFromEuler(
self.food_orient_eul, physicsClientId=self.id)
mesh_scale = np.array(
[self.food_base_scale[self.food_type]] * 3) * self.food_scale
food_visual = p.createVisualShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(self.world_creation.directory, 'food_items',
self.foods[self.food_type] + ".obj"),
rgbaColor=[1.0, 1.0, 1.0, 1.0],
meshScale=mesh_scale,
physicsClientId=self.id)
food_collision = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(self.world_creation.directory, 'food_items',
self.foods[self.food_type] + "_vhacd.obj"),
meshScale=mesh_scale,
physicsClientId=self.id)
self.food_item = p.createMultiBody(
baseMass=0.012,
baseCollisionShapeIndex=food_collision,
baseVisualShapeIndex=food_visual,
basePosition=[0, 0, 0],
useMaximalCoordinates=False,
physicsClientId=self.id)
p.changeVisualShape(
self.food_item,
-1,
textureUniqueId=p.loadTexture(
os.path.join(self.world_creation.directory, 'food_items',
self.foods[self.food_type] + ".png")))
# Disable collisions between the tool and food item
for ti in list(
range(p.getNumJoints(self.drop_fork,
physicsClientId=self.id))) + [-1]:
for tj in list(
range(
p.getNumJoints(self.food_item,
physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(self.drop_fork,
self.food_item,
ti,
tj,
False,
physicsClientId=self.id)
# Create constraint that keeps the food item in the tool
constraint = p.createConstraint(
self.drop_fork,
-1,
self.food_item,
-1,
p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, -0.025],
childFramePosition=self.child_offset,
parentFrameOrientation=self.food_orient_quat,
childFrameOrientation=[0, 0, 0, 1],
physicsClientId=self.id)
p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
# p.resetBasePositionAndOrientation(self.bowl, bowl_pos, p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.human, physicsClientId=self.id)
p.setPhysicsEngineParameter(numSubSteps=5,
numSolverIterations=10,
physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
# init_pos = [0.0, 0.0, 0.0, -2 * np.pi / 4, 0.0, np.pi / 2, np.pi / 4, 0.0, 0.0, 0.05, 0.05]
# self._reset_robot(init_pos)
# initialize images
for i in range(10):
p.stepSimulation()
ee_pos, ee_orient = p.getBasePositionAndOrientation(self.food_item)
self.viewMat1 = p.computeViewMatrixFromYawPitchRoll(
ee_pos, self.camdist, self.yaw, self.pitch, self.roll, 2, self.id)
self.viewMat2 = p.computeViewMatrixFromYawPitchRoll(
ee_pos, self.camdist, -self.yaw, self.pitch, self.roll, 2, self.id)
self.projMat = p.computeProjectionMatrixFOV(
self.fov, self.img_width / self.img_height, self.near, self.far)
images1 = p.getCameraImage(self.img_width,
self.img_height,
self.viewMat1,
self.projMat,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
physicsClientId=self.id)
images2 = p.getCameraImage(self.img_width,
self.img_height,
self.viewMat2,
self.projMat,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
physicsClientId=self.id)
self.rgb_opengl1 = np.reshape(
images1[2], (self.img_width, self.img_height, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images1[3],
[self.img_width, self.img_height])
self.depth_opengl1 = self.far * self.near / (
self.far - (self.far - self.near) * depth_buffer_opengl)
self.rgb_opengl2 = np.reshape(
images2[2], (self.img_width, self.img_height, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images2[3],
[self.img_width, self.img_height])
self.depth_opengl2 = self.far * self.near / (
self.far - (self.far - self.near) * depth_buffer_opengl)
if self.gui:
# TODO: do we need to update this every step? prob not
self.im1.set_data(self.rgb_opengl1)
self.im2.set_data(self.rgb_opengl2)
self.im1_d.set_data(self.depth_opengl1)
self.im2_d.set_data(self.depth_opengl2)
print(np.min(self.depth_opengl1), np.max(self.depth_opengl1))
self.fig.canvas.draw()
return self._get_obs(ret_images=ret_images, ret_dict=ret_dict)
because the simulation will crash.
Need to create different files for each
object in the scene
'''
targetID = p.loadURDF("URDF-target.xml",
flags=p.URDF_USE_SELF_COLLISION
or p.URDF_USE_SELF_COLLISION_INCLUDE_PARENT)
boxID = p.loadURDF("URDF-box.xml")
robotID = p.loadURDF("URDF-robot_arm.xml", useFixedBase=True)
p.changeDynamics(boxID,
-1,
lateralFriction=.3,
spinningFriction=.3,
rollingFriction=.3)
p.setCollisionFilterPair(boxID, targetID, -1, -1, 0)
num_joints = p.getNumJoints(robotID)
for i in range(num_joints):
x = p.getJointInfo(robotID, i)
print(
"-------------------------------------------------------------------------------------------------"
)
print("ID: ", x[0], " || Name: ", x[1], " || Type: ", x[2], " || qIndex: ",
x[3], " || uIndex ", x[4])
print("Flags: ", x[5], " || jointDamping: ", x[6], " || jointFriction: ",
x[7], " || jointLowerLimit: ", x[8])
print("JointUpperLimit: ", x[9], " || JointMaxForce: ", x[10],
" || jointMaxVelocity: ", x[11])
print("linkName: ", x[12], " || jointAxis: ", x[13])
def main(args):
pull_cfg_file = os.path.join(args.example_config_path, 'pull') + ".yaml"
pull_cfg = get_cfg_defaults()
pull_cfg.merge_from_file(pull_cfg_file)
pull_cfg.freeze()
grasp_cfg_file = os.path.join(args.example_config_path, 'grasp') + ".yaml"
grasp_cfg = get_cfg_defaults()
grasp_cfg.merge_from_file(grasp_cfg_file)
grasp_cfg.freeze()
# cfg = grasp_cfg
cfg = pull_cfg
rospy.init_node('MultiStep')
signal.signal(signal.SIGINT, signal_handler)
data_seed = args.np_seed
np.random.seed(data_seed)
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={'gui': args.visualize},
arm_cfg={
'self_collision': False,
'seed': data_seed
})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
p.changeDynamics(yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
yumi_gs = YumiGelslimPybulet(yumi_ar, cfg, exec_thread=False)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path + 'descriptions/urdf/' + args.object_name +
'.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:])
goal_obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path + 'descriptions/urdf/' + args.object_name +
'_trans.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:])
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
goal_obj_id,
r_gel_id,
-1,
enableCollision=False)
p.setCollisionFilterPair(obj_id,
goal_obj_id,
-1,
-1,
enableCollision=False)
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
obj_id,
r_gel_id,
-1,
enableCollision=True)
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
obj_id,
27,
-1,
enableCollision=True)
yumi_ar.pb_client.reset_body(obj_id, cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
yumi_ar.pb_client.reset_body(goal_obj_id, cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
# manipulated_object = None
# object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
# object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
# palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
# palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
# example_args = {}
# example_args['object_pose1_world'] = object_pose1_world
# example_args['object_pose2_world'] = object_pose2_world
# example_args['palm_pose_l_object'] = palm_pose_l_object
# example_args['palm_pose_r_object'] = palm_pose_r_object
# example_args['object'] = manipulated_object
# example_args['N'] = 60
# example_args['init'] = True
# example_args['table_face'] = 0
primitive_name = args.primitive
face = np.random.randint(6)
# face = 3
mesh_file = args.config_package_path + 'descriptions/meshes/objects/cuboids/' + \
args.object_name + '_experiments.stl'
cuboid_fname = mesh_file
exp_single = SingleArmPrimitives(pull_cfg,
yumi_ar.pb_client.get_client_id(), obj_id,
mesh_file)
exp_double = DualArmPrimitives(grasp_cfg,
yumi_ar.pb_client.get_client_id(),
obj_id,
mesh_file,
goal_face=face)
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
replan=args.replan,
object_mesh_file=mesh_file)
trans_box_lock = threading.RLock()
goal_viz = GoalVisual(trans_box_lock, goal_obj_id,
action_planner.pb_client, cfg.OBJECT_POSE_3)
multistep_planner = MultiStepManager(cfg, grasp_cfg, pull_cfg)
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
exp_double.initialize_object(obj_id, cuboid_fname, face)
print('Reset multi block!')
for _ in range(args.num_obj_samples):
# get grasp sample
# start_face_index = np.random.randint(len(grasp_cfg.VALID_GRASP_PAIRS[face]))
# start_face = grasp_cfg.VALID_GRASP_PAIRS[face][start_face_index]
# grasp_args = exp_double.get_random_primitive_args(ind=start_face,
# random_goal=True,
# execute=False)
# # pull_args_start = exp_single.get_random_primitive_args(ind=cfg.GRASP_TO_PULL[start_face],
# random_goal=True)
# pull_args_goal = exp_single.get_random_primitive_args(ind=cfg.GRASP_TO_PULL[face],
# random_goal=True)
# pull_args_start['object_pose2_world'] = grasp_args['object_pose1_world']
# pull_args_goal['object_pose1_world'] = grasp_args['object_pose2_world']
# full_args = [pull_args_start, grasp_args, pull_args_goal]
# full_args = [grasp_args]
full_args = multistep_planner.get_args(exp_single,
exp_double,
face,
execute=False)
# obj_pose_final = exp_running.goal_pose_world_frame_mod
# palm_poses_obj_frame = {}
# y_normals = action_planner.get_palm_y_normals(palm_poses_world)
# delta = 2e-3
# for key in palm_poses_world.keys():
# palm_pose_world = palm_poses_world[key]
# # try to penetrate the object a small amount
# palm_pose_world.pose.position.x -= delta*y_normals[key].pose.position.x
# palm_pose_world.pose.position.y -= delta*y_normals[key].pose.position.y
# palm_pose_world.pose.position.z -= delta*y_normals[key].pose.position.z
# palm_poses_obj_frame[key] = util.convert_reference_frame(
# palm_pose_world, obj_pose_world, util.unit_pose())
valid_subplans = 0
valid_plans = []
for plan_args in full_args:
plan = action_planner.get_primitive_plan(plan_args['name'],
plan_args, 'right')
goal_viz.update_goal_state(
util.pose_stamped2list(plan_args['object_pose2_world']))
start_pose = plan_args['object_pose1_world']
goal_pose = plan_args['object_pose2_world']
if args.debug:
for _ in range(10):
simulation.visualize_object(
start_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_initial",
color=(1., 0., 0., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
simulation.visualize_object(
goal_pose,
filepath=
"package://config/descriptions/meshes/objects/cuboids/"
+ cuboid_fname.split('objects/cuboids')[1],
name="/object_final",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
rospy.sleep(.1)
simulation.simulate(plan,
cuboid_fname.split('objects/cuboids')[1])
else:
primitive_name = plan_args['name']
start_joints = {}
if primitive_name == 'pull':
start_joints['right'] = pull_cfg.RIGHT_INIT
start_joints['left'] = pull_cfg.LEFT_INIT
elif primitive_name == 'grasp':
start_joints['right'] = grasp_cfg.RIGHT_INIT
start_joints['left'] = grasp_cfg.LEFT_INIT
subplan_valid = action_planner.full_mp_check(
plan, primitive_name, start_joints=start_joints)
if subplan_valid:
print("subplan valid!")
valid_subplans += 1
valid_plans.append(plan)
else:
print("not valid, primitive: " + primitive_name)
if valid_subplans == len(full_args) and not args.debug:
yumi_ar.pb_client.reset_body(
obj_id,
util.pose_stamped2list(full_args[0]['object_pose1_world'])[:3],
util.pose_stamped2list(full_args[0]['object_pose1_world'])[3:])
for plan_args in full_args:
# teleport to start state, to avoid colliding with object during transit
primitive_name = plan_args['name']
time.sleep(0.1)
for _ in range(10):
if primitive_name == 'pull':
# yumi_gs.update_joints(pull_cfg.RIGHT_INIT + pull_cfg.LEFT_INIT)
yumi_gs.yumi_pb.arm.set_jpos(pull_cfg.RIGHT_INIT +
pull_cfg.LEFT_INIT,
ignore_physics=True)
elif primitive_name == 'grasp':
# yumi_gs.update_joints(grasp_cfg.RIGHT_INIT + grasp_cfg.LEFT_INIT)
yumi_gs.yumi_pb.arm.set_jpos(grasp_cfg.RIGHT_INIT +
grasp_cfg.LEFT_INIT,
ignore_physics=True)
try:
# should update the plan args start state to be the current simulator state
result = action_planner.execute(primitive_name, plan_args)
if result is not None:
print(str(result[0]))
except ValueError as e:
print("Value error: ")
print(e)
time.sleep(1.0)
if primitive_name == 'pull':
pose = util.pose_stamped2list(
yumi_gs.compute_fk(yumi_gs.get_jpos(arm='right')))
pos, ori = pose[:3], pose[3:]
# pose = yumi_gs.get_ee_pose()
# pos, ori = pose[0], pose[1]
# pos[2] -= 0.0714
pos[2] += 0.001
r_jnts = yumi_gs.compute_ik(pos, ori,
yumi_gs.get_jpos(arm='right'))
l_jnts = yumi_gs.get_jpos(arm='left')
if r_jnts is not None:
for _ in range(10):
pos[2] += 0.001
r_jnts = yumi_gs.compute_ik(
pos, ori, yumi_gs.get_jpos(arm='right'))
l_jnts = yumi_gs.get_jpos(arm='left')
if r_jnts is not None:
yumi_gs.update_joints(list(r_jnts) + l_jnts)
time.sleep(0.1)
time.sleep(0.1)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
def start():
p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())
plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
# p.setDefaultContactERP(0)
# urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago_toes.urdf", [0, 0, .5],
[0, 0.5, 0.5, 0],
flags=urdfFlags,
useFixedBase=False)
# enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
# 2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=",
enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5,
enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
# print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
jointIds.append(j)
p.getCameraImage(480, 320)
p.setRealTimeSimulation(0)
joints = []
with open(pd.getDataPath() + "/laikago/data1.txt", "r") as filestream:
for line in filestream:
print("line=", line)
maxForce = p.readUserDebugParameter(maxForceId)
currentline = line.split(",")
# print (currentline)
# print("-----")
frame = currentline[0]
t = currentline[1]
# print("frame[",frame,"]")
joints = currentline[2:14]
# print("joints=",joints)
for j in range(12):
targetPos = float(joints[j])
p.setJointMotorControl2(quadruped,
jointIds[j],
p.POSITION_CONTROL,
jointDirections[j] * targetPos +
jointOffsets[j],
force=maxForce)
p.stepSimulation()
for lower_leg in lower_legs:
# print("points for ", quadruped, " link: ", lower_leg)
pts = p.getContactPoints(quadruped, -1, lower_leg)
# print("num points=",len(pts))
# for pt in pts:
# print(pt[9])
time.sleep(1. / 500.)
index = 0
for j in range(p.getNumJoints(quadruped)):
p.changeDynamics(quadruped, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(quadruped, j)
js = p.getJointState(quadruped, j)
# print(info)
jointName = info[1]
jointType = info[2]
if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
paramIds.append(
p.addUserDebugParameter(jointName.decode("utf-8"), -4, 4,
(js[0] - jointOffsets[index]) /
jointDirections[index]))
index = index + 1
p.setRealTimeSimulation(1)
while (1):
for i in range(len(paramIds)):
c = paramIds[i]
targetPos = p.readUserDebugParameter(c)
maxForce = p.readUserDebugParameter(maxForceId)
p.setJointMotorControl2(quadruped,
jointIds[i],
p.POSITION_CONTROL,
jointDirections[i] * targetPos +
jointOffsets[i],
force=maxForce)
def __init__(self,
max_steps=500,
seed=1234,
gui=False,
map_width=3,
chamber_fraction=1 / 3,
observation_height=7,
render_height=15,
iters=2000,
render_shape=(128 * 2, 128 * 2, 3),
observation_shape=(10, 10, 1),
obs_fov=45,
obs_aspect=1.0,
obs_nearplane=0.01,
obs_farplane=100,
reset_upon_touch=False,
touch_thresh=1.,
n_particles=2,
observation_stack=2,
flat_obs=True,
grayscale=True,
fixed_view=True,
agent_scaling=0.5,
dt=1 / 50.0,
include_true_state=False):
super(TagEnv, self).__init__()
from collections import deque
self._include_true_state = include_true_state
self._GRAVITY = -9.8
self._dt = dt
self.sim_steps = 5
# Temp. Doesn't currently make sense if smaller.
assert (map_width >= 2.)
self._map_area = self._map_width**2
# This constant doesn't affect the created width.
self._cube_width = 1.
self.dt = self._dt
self._game_settings = {"include_egocentric_vision": True}
# self.action_space = gym.spaces.Box(low=np.array([-1.2, -1.2, 0]), high=np.array([1.2,1.2,1]))
if gui:
# self._object.setPosition([self._x[self._step], self._y[self._step], 0.0] )
self._physicsClient = pybullet.connect(pybullet.GUI)
else:
self._physicsClient = pybullet.connect(pybullet.DIRECT)
pybullet.configureDebugVisualizer(pybullet.COV_ENABLE_SHADOWS,
0,
lightPosition=[0, 0, 10])
RLSIMENV_PATH = os.environ['RLSIMENV_PATH']
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.resetSimulation()
#pybullet.setRealTimeSimulation(True)
pybullet.setGravity(0, 0, self._GRAVITY)
pybullet.setPhysicsEngineParameter(fixedTimeStep=self._dt)
pybullet.setTimeStep(self._dt)
# _planeId = pybullet.loadURDF("plane.urdf", )
cubeStartPos = [0, 0, 0.5]
cubeStartOrientation = pybullet.getQuaternionFromEuler([0., 0, 0])
# These exist as part of the pybullet installation.
self._demon = pybullet.loadURDF(DATA_DIR + "/sphere2_yellow.urdf",
cubeStartPos,
cubeStartOrientation,
useFixedBase=0,
globalScaling=agent_scaling)
self._particles = []
self._particle_states = []
for _ in range(self._n_particles):
self._particles.append(
pybullet.loadURDF(DATA_DIR + "/sphere2_red2.urdf",
cubeStartPos,
cubeStartOrientation,
useFixedBase=0))
self._particle_states.append({"fixed": False, "drag": 1.0})
pybullet.setCollisionFilterPair(self._demon,
self._particles[-1],
-1,
-1,
enableCollision=0)
self._boxes = []
for _ in range(0):
self._boxes.append(
pybullet.loadURDF(DATA_DIR + "/cube3.urdf",
cubeStartPos,
cubeStartOrientation,
useFixedBase=0))
pybullet.setCollisionFilterPair(self._demon,
self._boxes[-1],
-1,
-1,
enableCollision=0)
pybullet.changeVisualShape(self._boxes[-1],
-1,
rgbaColor=[0.2, 0.2, 0.8, 1.0])
pybullet.setAdditionalSearchPath(RLSIMENV_PATH + '/rlsimenv/data')
# Add walls
self._blocks = []
self._wall_heights = [0.5, 1.5]
self.action_space = gym.spaces.Box(
low=np.array([-2.5, -2.5, 0]),
high=np.array([2.5, 2.5, 1])) #self._wall_heights[-1]]))
# self._wall_heights = (0.5, 1.5)
x_full_right = (
1 + self._chamber_fraction) * self._map_width + self._cube_width
x_inner_right = self._map_width
for z in self._wall_heights:
cube_locations = []
# Right walls
# cube_locations.extend([[self._map_width, y, z] for y in range(-self._map_width, -2)])
# cube_locations.extend([[self._map_width, y, z] for y in range(2, self._map_width)])
# Right wall
cube_locations.extend(
[[self._map_width, y, z]
for y in np.arange(-self._map_width, self._map_width)])
# Left wall
cube_locations.extend(
[[-self._map_width, y, z]
for y in range(-self._map_width, self._map_width)])
# Top Wall
cube_locations.extend(
[[x, self._map_width, z]
for x in np.arange(-self._map_width, self._map_width)])
# Bottom Wall
cube_locations.extend(
[[x, -self._map_width, z]
for x in np.arange(-self._map_width, self._map_width)])
# Add small room
# Add Right wall
# "Small" small room
# cube_locations.extend([[self._map_width+(self._map_width//2), y, z] for y in range(-self._map_width//2, self._map_width//2)])
# cube_locations.extend([[self._map_width, y, z] for y in range(-self._map_width, self._map_width)])
# Top wall
# cube_locations.extend([[x, self._map_width//2, z] for x in range(self._map_width, self._map_width+(self._map_width//2))])
# Bottom wall
# cube_locations.extend([[x, -self._map_width//2, z] for x in range(self._map_width, self._map_width+(self._map_width//2))])
# print ("cube_locations: ", cube_locations)
for loc in cube_locations:
blockId = pybullet.loadURDF(DATA_DIR + "/cube3.urdf",
loc,
cubeStartOrientation,
useFixedBase=1)
self._blocks.append(blockId)
### We can get away with a single box.
self._roof = []
self._roof.append(
pybullet.loadURDF(DATA_DIR + "/cube2.urdf", [1, 0, 7.0],
cubeStartOrientation,
useFixedBase=1,
globalScaling=10))
pybullet.changeVisualShape(self._roof[-1],
-1,
rgbaColor=[.4, .4, .4, 0.0])
for body in self._particles + self._blocks + self._roof:
pybullet.changeDynamics(body,
-1,
rollingFriction=0.,
spinningFriction=0.0,
lateralFriction=0.0,
linearDamping=0.0,
angularDamping=0.0,
restitution=1.0,
maxJointVelocity=10)
pybullet.loadURDF(DATA_DIR + "/white_plane.urdf", [0, 0, -0.05],
useFixedBase=1)
# disable the default velocity motors
#and set some position control with small force to emulate joint friction/return to a rest pose
jointFrictionForce = 1
for joint in range(pybullet.getNumJoints(self._demon)):
pybullet.setJointMotorControl2(self._demon,
joint,
pybullet.POSITION_CONTROL,
force=jointFrictionForce)
#for i in range(10000):
# pybullet.setJointMotorControl2(botId, 1, pybullet.TORQUE_CONTROL, force=1098.0)
# pybullet.stepSimulation()
#import ipdb
#ipdb.set_trace()
pybullet.setRealTimeSimulation(1)
observation_stack
self._obs_stack = [[0], [1]]
# lo = self.getObservation()["pixels"] * 0.0
# hi = lo + 1.0
lo = np.zeros(
(np.prod(self._observation_shape) * self._observation_stack))
hi = np.ones(
(np.prod(self._observation_shape) * self._observation_stack))
self._game_settings['state_bounds'] = [lo, hi]
self._obs_stack = deque()
for _ in range(self._observation_stack):
self._obs_stack.append(np.zeros(np.prod(self._observation_shape)))
# self._obs_stack = [lo] * self._observation_stack
# self._observation_space = ActionSpace(self._game_settings['state_bounds'])
if (self._flat_obs):
self.observation_space = gym.spaces.Box(low=lo, high=hi)
else:
self.observation_space = gym.spaces.Box(
low=0, high=1, shape=self._observation_shape)
self._vel_bounds = [[-2.5, -2.5, -2.5], [2.5, 2.5, 2.5]]
def setup_simulation(self):
""" Setup the simulation. """
########## PYBULLET SETUP ##########
if self.record_movie and self.gui == p.GUI:
p.connect(self.gui,
options=('--background_color_red={}'
' --background_color_green={}'
' --background_color_blue={}'
' --mp4={}'
' --mp4fps={}').format(
self.vis_options_background_color_red,
self.vis_options_background_color_green,
self.vis_options_background_color_blue,
self.movie_name,
int(self.movie_speed / self.time_step)))
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING, 1)
elif self.gui == p.GUI:
p.connect(self.gui,
options=(' --background_color_red={}'
' --background_color_green={}'
' --background_color_blue={}').format(
self.vis_options_background_color_red,
self.vis_options_background_color_green,
self.vis_options_background_color_blue))
else:
p.connect(self.gui)
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Everything should fall down
p.setGravity(*[g * self.units.gravity for g in self.gravity])
p.setPhysicsEngineParameter(fixedTimeStep=self.time_step,
numSolverIterations=self.solver_iterations,
numSubSteps=self.num_substep,
solverResidualThreshold=1e-10,
erp=0.0,
contactERP=self.contactERP,
frictionERP=0.0,
globalCFM=self.globalCFM,
reportSolverAnalytics=1)
# Turn off rendering while loading the models
self.rendering(0)
if self.ground == 'floor':
# Add floor
self.plane = p.loadURDF('plane.urdf', [0, 0, -0.],
globalScaling=0.01 * self.units.meters)
# When plane is used the link id is -1
self.link_plane = -1
p.changeDynamics(self.plane,
-1,
lateralFriction=self.ground_friction_coef)
self.sim_data.add_table('base_linear_velocity')
self.sim_data.add_table('base_angular_velocity')
self.sim_data.add_table('base_orientation')
elif self.ground == 'ball':
# Add floor and ball
self.floor = p.loadURDF('plane.urdf', [0, 0, -0.],
globalScaling=0.01 * self.units.meters)
if self.ball_info:
self.ball_radius, ball_pos = self.load_ball_info()
else:
ball_pos = None
self.plane = self.add_ball(self.ball_radius, self.ball_density,
self.ball_mass,
self.ground_friction_coef, ball_pos)
# When ball is used the plane id is 2 as the ball has 3 links
self.link_plane = 2
self.sim_data.add_table('ball_rotations')
self.sim_data.add_table('ball_velocity')
# Add the animal model
if '.sdf' in self.model and self.behavior is not None:
self.animal, self.link_id, self.joint_id = load_sdf(
self.model, force_concave=self.enable_concave_mesh)
elif '.sdf' in self.model and self.behavior is None:
self.animal = p.loadSDF(self.model)[0]
# Generate joint_name to id dict
self.link_id[p.getBodyInfo(self.animal)[0].decode('UTF-8')] = -1
for n in range(p.getNumJoints(self.animal)):
info = p.getJointInfo(self.animal, n)
_id = info[0]
joint_name = info[1].decode('UTF-8')
link_name = info[12].decode('UTF-8')
_type = info[2]
self.joint_id[joint_name] = _id
self.joint_type[joint_name] = _type
self.link_id[link_name] = _id
elif '.urdf' in self.model:
self.animal = p.loadURDF(self.model)
p.resetBasePositionAndOrientation(
self.animal, self.model_offset,
p.getQuaternionFromEuler([0., 0., 0.]))
self.num_joints = p.getNumJoints(self.animal)
# Body colors
color_wings = [91 / 100, 96 / 100, 97 / 100, 0.7]
color_eyes = [67 / 100, 21 / 100, 12 / 100, 1]
self.color_body = [140 / 255, 100 / 255, 30 / 255, 1]
self.color_legs = [170 / 255, 130 / 255, 50 / 255, 1]
self.color_collision = [0, 1, 0, 1]
nospecular = [0.5, 0.5, 0.5]
# Color the animal
p.changeVisualShape(self.animal,
-1,
rgbaColor=self.color_body,
specularColor=nospecular)
for link_name, _id in self.joint_id.items():
if 'Wing' in link_name and 'Fake' not in link_name:
p.changeVisualShape(self.animal, _id, rgbaColor=color_wings)
elif 'Eye' in link_name and 'Fake' not in link_name:
p.changeVisualShape(self.animal, _id, rgbaColor=color_eyes)
# and 'Fake' not in link_name:
elif ('Tarsus' in link_name or 'Tibia' in link_name
or 'Femur' in link_name or 'Coxa' in link_name):
p.changeVisualShape(self.animal,
_id,
rgbaColor=self.color_legs,
specularColor=nospecular)
elif 'Fake' not in link_name:
p.changeVisualShape(self.animal,
_id,
rgbaColor=self.color_body,
specularColor=nospecular)
# print('Link name {} id {}'.format(link_name, _id))
# Configure contacts
# Disable/Enable all self-collisions
for link0 in self.link_id.keys():
for link1 in self.link_id.keys():
p.setCollisionFilterPair(
bodyUniqueIdA=self.animal,
bodyUniqueIdB=self.animal,
linkIndexA=self.link_id[link0],
linkIndexB=self.link_id[link1],
enableCollision=0,
)
# Disable/Enable tarsi-ground collisions
for link in self.link_id.keys():
if 'Tarsus' in link:
p.setCollisionFilterPair(bodyUniqueIdA=self.animal,
bodyUniqueIdB=self.plane,
linkIndexA=self.link_id[link],
linkIndexB=self.link_plane,
enableCollision=1)
# Disable/Enable selected self-collisions
for (link0, link1) in self.self_collisions:
p.setCollisionFilterPair(
bodyUniqueIdA=self.animal,
bodyUniqueIdB=self.animal,
linkIndexA=self.link_id[link0],
linkIndexB=self.link_id[link1],
enableCollision=1,
)
# ADD container columns
# ADD ground reaction forces and friction forces
_ground_sensor_ids = []
for contact in self.ground_contacts:
_ground_sensor_ids.append((self.animal, self.plane,
self.link_id[contact], self.link_plane))
self.sim_data.contact_flag.add_parameter(f"{contact}_flag")
for axis in ('x', 'y', 'z'):
self.sim_data.contact_position.add_parameter(contact + '_' +
axis)
self.sim_data.contact_normal_force.add_parameter(contact +
'_' + axis)
self.sim_data.contact_lateral_force.add_parameter(contact +
'_' + axis)
# ADD self collision forces
_collision_sensor_ids = []
for link0, link1 in self.self_collisions:
_collision_sensor_ids.append(
(self.animal, self.animal, self.link_id[link0],
self.link_id[link1]))
contacts = '-'.join((link0, link1))
for axis in ['x', 'y', 'z']:
self.sim_data.contact_position.add_parameter(contacts + '_' +
axis)
self.sim_data.contact_normal_force.add_parameter(contacts +
'_' + axis)
self.sim_data.contact_lateral_force.add_parameter(contacts +
'_' + axis)
# Generate sensors
self.joint_sensors = JointSensors(self.animal,
self.sim_data,
meters=self.units.meters,
velocity=self.units.velocity,
torques=self.units.torques)
self.contact_sensors = ContactSensors(
self.sim_data,
tuple([*_ground_sensor_ids, *_collision_sensor_ids]),
meters=self.units.meters,
newtons=self.units.newtons)
self.com_sensor = COMSensor(self.animal,
self.sim_data,
meters=self.units.meters,
kilograms=self.units.kilograms)
# ADD base position parameters
for axis in ['x', 'y', 'z']:
self.sim_data.base_position.add_parameter(f'{axis}')
# self.sim_data.thorax_force.add_parameter(f'{axis}')
if self.ground == 'ball':
self.sim_data.ball_rotations.add_parameter(f'{axis}')
self.sim_data.ball_velocity.add_parameter(f'{axis}')
if self.ground == 'floor':
self.sim_data.base_linear_velocity.add_parameter(f'{axis}')
self.sim_data.base_angular_velocity.add_parameter(f'{axis}')
self.sim_data.base_orientation.add_parameter(f'{axis}')
# ADD joint parameters
for name, _ in self.joint_id.items():
self.sim_data.joint_positions.add_parameter(name)
self.sim_data.joint_velocities.add_parameter(name)
self.sim_data.joint_torques.add_parameter(name)
# ADD Center of mass parameters
for axis in ('x', 'y', 'z'):
self.sim_data.center_of_mass.add_parameter(f"{axis}")
# ADD muscles
if self.use_muscles:
self.initialize_muscles()
# ADD controller
if self.controller_config:
self.controller = NeuralSystem(
config_path=self.controller_config,
container=self.container,
)
# Disable default bullet controllers
p.setJointMotorControlArray(self.animal,
np.arange(self.num_joints),
p.VELOCITY_CONTROL,
targetVelocities=np.zeros(
(self.num_joints, 1)),
forces=np.zeros((self.num_joints, 1)))
p.setJointMotorControlArray(self.animal,
np.arange(self.num_joints),
p.POSITION_CONTROL,
forces=np.zeros((self.num_joints, 1)))
p.setJointMotorControlArray(self.animal,
np.arange(self.num_joints),
p.TORQUE_CONTROL,
forces=np.zeros((self.num_joints, 1)))
# Disable link linear and angular damping
for njoint in range(self.num_joints):
p.changeDynamics(self.animal, njoint, linearDamping=0.0)
p.changeDynamics(self.animal, njoint, angularDamping=0.0)
p.changeDynamics(self.animal, njoint, jointDamping=0.0)
self.total_mass = 0.0
for j in np.arange(-1, p.getNumJoints(self.animal)):
self.total_mass += p.getDynamicsInfo(self.animal,
j)[0] / self.units.kilograms
self.bodyweight = -1 * self.total_mass * self.gravity[2]
print('Total mass = {}'.format(self.total_mass))
if self.gui == p.GUI:
self.rendering(1)
def __init__(self, action_repeat=10, render=False):
self._action_repeat = action_repeat
self.spec = None
self.robot = Robot('ur5e_stick',
pb=True,
pb_cfg={
'gui': render,
'realtime': False
})
self.ee_ori = [-np.sqrt(2) / 2, np.sqrt(2) / 2, 0, 0]
self._action_bound = 1.0
self._ee_pos_scale = 0.02
self._ee_ori_scale = np.pi / 36.0
self._action_high = np.array([self._action_bound] * 2)
self.action_space = spaces.Box(low=-self._action_high,
high=self._action_high,
dtype=np.float32)
self.goal = np.array([0.75, -0.3, 1.0])
self.init = np.array([0.5, 0.3, 1.0])
self.init_obj = np.array([0.5, 0.2, 1.0])
self.push_len = 0.5
self.push_ang_range = np.pi / 3
self.robot.arm.reset()
ori = euler2quat([0, 0, np.pi / 2])
self.table_id = self.robot.pb_client.load_urdf('table/table.urdf',
[.5, 0, 0.4],
ori,
scaling=0.9)
self.obj_id = self.robot.pb_client.load_geom('cylinder',
size=[0.08, 0.05],
mass=1,
base_pos=self.init_obj,
rgba=[1, 0, 0, 1])
self.marker_id = self.robot.pb_client.load_geom('box',
size=0.05,
mass=1,
base_pos=self.goal,
rgba=[0, 1, 0, 0.4])
client_id = self.robot.pb_client.get_client_id()
p.setCollisionFilterGroupMask(self.marker_id,
-1,
0,
0,
physicsClientId=client_id)
p.setCollisionFilterPair(self.marker_id,
self.table_id,
-1,
-1,
1,
physicsClientId=client_id)
self.reset()
state_low = np.full(len(self._get_obs()), -float('inf'))
state_high = np.full(len(self._get_obs()), float('inf'))
self.observation_space = spaces.Box(state_low,
state_high,
dtype=np.float32)
def spawn_additional_objects(self):
ids = []
corr_l = self.np_random.uniform(*self.p["world"]["corridor_length"])
corr_w = self.np_random.uniform(*self.p["world"]["corridor_width"])
wall_w = self.np_random.uniform(*self.p["world"]["wall_width"])
wall_h = self.np_random.uniform(*self.p["world"]["wall_height"])
self.corridor_width = corr_w
self.corridor_length = corr_l
# Reset occupancy map
om_res = self.p['world']['HPF']['res']
self.occmap = OccupancyMap(0, corr_l, 0, corr_w, om_res)
# Spawn walls, row 1
pos = np.zeros(3)
while pos[0] < corr_l:
wall_l_i = self.np_random.uniform(*self.p["world"]["wall_length"])
door_l_i = self.np_random.uniform(*self.p["world"]["door_length"])
halfExtents = [wall_l_i/2, wall_w/2, wall_h/2]
colBoxId = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=halfExtents)
visBoxId = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=halfExtents)
pos_i = pos + np.array(halfExtents*np.array((1, -1, 1)))
id = pb.createMultiBody(0, colBoxId, visBoxId, pos_i)
ids.append(id)
# Create room walls
if self.np_random.uniform(0, 1) <= 0.5:
he_wall_w_w = self.np_random.uniform(0, 3)
he_wall_d_w = self.np_random.uniform(0, 3)
he_w = [wall_w/2, he_wall_w_w, wall_h/2]
he_d = [wall_w/2, he_wall_d_w, wall_h/2]
colBoxId_w = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=he_w)
visBoxId_w = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=he_w)
colBoxId_d = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=he_d)
visBoxId_d = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=he_d)
pos_i = pos + np.array(he_w*np.array((1, -1, 1)))
pos_i[0] += wall_l_i - wall_w
id_w = pb.createMultiBody(0, colBoxId_w, visBoxId_w, pos_i)
ids.append(id_w)
pos_i = pos + np.array(he_d*np.array((1, -1, 1)))
pos_i[0] += wall_l_i + door_l_i
id_d = pb.createMultiBody(0, colBoxId_d, visBoxId_d, pos_i)
ids.append(id_d)
pos += np.array((wall_l_i + door_l_i, 0, 0))
self.configure_ext_collisions(id, self.robotId, self.collision_links)
# Spawn walls, row 2
pos += np.array((0, corr_w, 0))
while pos[0] > 0:
wall_l_i = self.np_random.uniform(*self.p["world"]["wall_length"])
door_l_i = self.np_random.uniform(*self.p["world"]["door_length"])
halfExtents = [wall_l_i/2, wall_w/2, wall_h/2]
colBoxId = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=halfExtents)
visBoxId = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=halfExtents)
pos_i = pos + np.array(halfExtents*np.array((-1, 1, 1)))
id = pb.createMultiBody(0, colBoxId, visBoxId, pos_i)
ids.append(id)
# Create room walls
if self.np_random.uniform(0, 1) <= 0.5:
he_wall_w_w = self.np_random.uniform(0, 3)
he_wall_d_w = self.np_random.uniform(0, 3)
he_w = [wall_w/2, he_wall_w_w, wall_h/2]
he_d = [wall_w/2, he_wall_d_w, wall_h/2]
colBoxId_w = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=he_w)
visBoxId_w = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=he_w)
colBoxId_d = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=he_d)
visBoxId_d = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=he_d)
pos_i = pos + np.array(he_w*np.array((-1, 1, 1)))
pos_i[0] -= wall_l_i
id_w = pb.createMultiBody(0, colBoxId_w, visBoxId_w, pos_i)
ids.append(id_w)
pos_i = pos + np.array(he_d*np.array((-1, 1, 1)))
pos_i[0] -= wall_l_i + door_l_i
id_d = pb.createMultiBody(0, colBoxId_d, visBoxId_d, pos_i)
ids.append(id_d)
pos -= np.array((wall_l_i+door_l_i, 0, 0))
self.configure_ext_collisions(id, self.robotId, self.collision_links)
sg = SpawnGrid(corr_l*2, corr_w, res=0.01, min_dis=self.p["world"]["min_clearance"])
sg.add_shelf(4+1.47/2, 1.47, 0.39, 0)
sg.add_shelf(12+1.47/2, 1.47, 0.39, 0)
self.occmap.add_rect([4+1.47/2, 0.39/2], 1.47, 0.39)
self.occmap.add_rect([12+1.47/2, 0.39/2], 1.47, 0.39)
# Spawn shelves, row 1
pos = np.zeros(3)
while pos[0] < corr_l:
shlf_l_i = self.np_random.uniform(*self.p["world"]["shelf_length"])
mw = sg.get_max_width(pos[0]+shlf_l_i/2, shlf_l_i, 0)
width_lims = self.p["world"]["shelf_width"].copy()
width_lims[1] = min(width_lims[1], mw)
shlf_w_i = self.np_random.uniform(*width_lims)
shlf_h_i = self.np_random.uniform(*self.p["world"]["shelf_height"])
sgap_l_i = self.np_random.uniform(*self.p["world"]["shelf_gap"])
halfExtents = [shlf_l_i/2, shlf_w_i/2, shlf_h_i/2]
colBoxId = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=halfExtents)
visBoxId = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=halfExtents)
pos_i = pos + np.array(halfExtents*np.array((1, 1, 1)))
margin = 0.3
if abs(pos_i[0] - 4.735) >= 0.735 + halfExtents[0] + margin and \
abs(pos_i[0] - 12.735) >= 0.735 + halfExtents[0] + margin:
sg.add_shelf(pos[0]+shlf_l_i/2, shlf_l_i, shlf_w_i, 0)
id = pb.createMultiBody(0, colBoxId, visBoxId, pos_i)
ids.append(id)
self.occmap.add_rect([pos[0]+shlf_l_i/2.0, pos[1]+shlf_w_i/2],
shlf_l_i, shlf_w_i)
pos += np.array((shlf_l_i + sgap_l_i, 0, 0))
self.configure_ext_collisions(id, self.robotId, self.collision_links)
else:
pos += np.array((0.05, 0, 0))
# Spawn shelves, row 2
pos += np.array((0, corr_w, 0))
while pos[0] > 0:
shlf_l_i = self.np_random.uniform(*self.p["world"]["shelf_length"])
mw = sg.get_max_width(pos[0]-shlf_l_i/2, shlf_l_i, 1)
width_lims = self.p["world"]["shelf_width"].copy()
width_lims[1] = min(width_lims[1], mw)
shlf_w_i = self.np_random.uniform(*width_lims)
sg.add_shelf(pos[0]-shlf_l_i/2, shlf_l_i, shlf_w_i, 1)
shlf_h_i = self.np_random.uniform(*self.p["world"]["shelf_height"])
sgap_l_i = self.np_random.uniform(*self.p["world"]["shelf_gap"])
halfExtents = [shlf_l_i/2, shlf_w_i/2, shlf_h_i/2]
colBoxId = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=halfExtents)
visBoxId = pb.createCollisionShape(pb.GEOM_BOX,
halfExtents=halfExtents)
pos_i = pos + np.array(halfExtents*np.array((-1, -1, 1)))
id = pb.createMultiBody(0, colBoxId, visBoxId, pos_i)
ids.append(id)
self.occmap.add_rect([pos[0]-shlf_l_i/2.0, pos[1]-shlf_w_i/2],
shlf_l_i, shlf_w_i)
pos -= np.array((shlf_l_i+sgap_l_i, 0, 0))
self.configure_ext_collisions(id, self.robotId, self.collision_links)
for id in ids:
for human in self.humans:
pb.setCollisionFilterPair(human.leg_l, id, -1, -1, False,
self.clientId)
pb.setCollisionFilterPair(human.leg_r, id, -1, -1, False,
self.clientId)
# print(sg.matrix1)
# print(sg.matrix0)
# import matplotlib.pyplot as plt
# fig = plt.figure()
# ax = fig.add_subplot(111)
# ax.imshow(sg.matrix0.astype(float))
# plt.show()
return ids
def _setup_experiment(self):
super()._setup_experiment()
# disable collision since we're applying a force directly on the block (pusher is for visualization for now)
p.setCollisionFilterPair(self.pusherId, self.blockId, -1, -1, 0)
def worker_yumi(child_conn, work_queue, result_queue, cfg, args, seed,
worker_id, global_info_dict, worker_flag_dict, planning_lock):
while True:
try:
if not child_conn.poll(0.0001):
continue
msg = child_conn.recv()
except (EOFError, KeyboardInterrupt):
break
if msg == "RESET":
data_seed = seed
np.random.seed(data_seed)
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={'gui': args.visualize},
arm_cfg={
'self_collision': False,
'seed': data_seed
})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
p.changeDynamics(yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
dynamics_info = {}
dynamics_info['contactDamping'] = alpha * K
dynamics_info['contactStiffness'] = K
dynamics_info['rollingFriction'] = args.rolling
dynamics_info['restitution'] = restitution
yumi_gs = YumiCamsGS(yumi_ar,
cfg,
exec_thread=False,
sim_step_repeat=args.step_repeat)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '.urdf', cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:])
# goal_obj_id = yumi_ar.pb_client.load_urdf(
# args.config_package_path +
# 'descriptions/urdf/'+args.object_name+'_trans.urdf',
# cfg.OBJECT_POSE_3[0:3],
# cfg.OBJECT_POSE_3[3:]
# )
# p.setCollisionFilterPair(yumi_ar.arm.robot_id, goal_obj_id, r_gel_id, -1, enableCollision=False)
# p.setCollisionFilterPair(obj_id, goal_obj_id, -1, -1, enableCollision=False)
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
obj_id,
r_gel_id,
-1,
enableCollision=True)
p.setCollisionFilterPair(yumi_ar.arm.robot_id,
obj_id,
27,
-1,
enableCollision=True)
yumi_ar.pb_client.reset_body(obj_id, cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
# yumi_ar.pb_client.reset_body(
# goal_obj_id,
# cfg.OBJECT_POSE_3[:3],
# cfg.OBJECT_POSE_3[3:])
primitive_name = args.primitive
mesh_file = '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids/realsense_box_experiments.stl'
cuboid_fname = mesh_file
face = 0
# setup macro_planner
action_planner = ClosedLoopMacroActions(
cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
object_mesh_file=mesh_file,
replan=args.replan)
exp_single = SingleArmPrimitives(cfg,
yumi_ar.pb_client.get_client_id(),
obj_id, mesh_file)
exp_double = DualArmPrimitives(cfg,
yumi_ar.pb_client.get_client_id(),
obj_id, mesh_file)
if primitive_name == 'grasp' or primitive_name == 'pivot':
exp_running = exp_double
else:
exp_running = exp_single
action_planner.update_object(obj_id, mesh_file)
exp_single.initialize_object(obj_id, cuboid_fname)
exp_double.initialize_object(obj_id, cuboid_fname, face)
# goal_viz = GoalVisual(
# trans_box_lock,
# goal_obj_id,
# action_planner.pb_client,
# cfg.OBJECT_POSE_3)
pickle_path = os.path.join(args.data_dir, primitive_name,
args.experiment_name)
if not os.path.exists(pickle_path):
os.makedirs(pickle_path)
data_manager = DataManager(pickle_path)
continue
if msg == "HOME":
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
continue
if msg == "OBJECT_POSE":
continue
if msg == "SAMPLE":
global_info_dict['trial'] += 1
# print('Success: ' + str(global_info_dict['success']) +
# ' Trial number: ' + str(global_info_dict['trial']) +
# ' Worker ID: ' + str(worker_id))
worker_flag_dict[worker_id] = False
success = False
start_face = 1
plan_args = exp_running.get_random_primitive_args(
ind=start_face, random_goal=False, execute=True)
try:
obs, pcd = yumi_gs.get_observation(obj_id=obj_id)
obj_pose_world = plan_args['object_pose1_world']
obj_pose_final = plan_args['object_pose2_world']
contact_obj_frame, contact_world_frame = {}, {}
contact_obj_frame['right'] = plan_args['palm_pose_r_object']
contact_world_frame['right'] = plan_args['palm_pose_r_world']
contact_obj_frame['left'] = plan_args['palm_pose_l_object']
contact_world_frame['left'] = plan_args['palm_pose_l_world']
start = util.pose_stamped2list(obj_pose_world)
goal = util.pose_stamped2list(obj_pose_final)
keypoints_start = np.array(
exp_running.mesh_world.vertices.tolist())
keypoints_start_homog = np.hstack(
(keypoints_start, np.ones((keypoints_start.shape[0], 1))))
start_mat = util.matrix_from_pose(obj_pose_world)
goal_mat = util.matrix_from_pose(obj_pose_final)
T_mat = np.matmul(goal_mat, np.linalg.inv(start_mat))
keypoints_goal = np.matmul(T_mat,
keypoints_start_homog.T).T[:, :3]
contact_obj_frame_dict = {}
contact_world_frame_dict = {}
nearest_pt_world_dict = {}
corner_norms = {}
down_pcd_norms = {}
if primitive_name == 'pull':
# active_arm, inactive_arm = action_planner.get_active_arm(
# util.pose_stamped2list(obj_pose_world)
# )
active_arm = action_planner.active_arm
inactive_arm = action_planner.inactive_arm
contact_obj_frame_dict[
active_arm] = util.pose_stamped2list(
contact_obj_frame[active_arm])
contact_world_frame_dict[
active_arm] = util.pose_stamped2list(
contact_world_frame[active_arm])
# contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[active_arm][:3]))
# kdtree = open3d.geometry.KDTreeFlann(pcd)
# nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0]
# nearest_pt_world_dict[active_arm] = np.asarray(pcd.points)[nearest_pt_ind]
contact_pos = np.array(
contact_world_frame_dict[active_arm][:3])
corner_dists = (np.asarray(keypoints_start) - contact_pos)
corner_norms[active_arm] = np.linalg.norm(corner_dists,
axis=1)
down_pcd_dists = (obs['down_pcd_pts'] - contact_pos)
down_pcd_norms[active_arm] = np.linalg.norm(down_pcd_dists,
axis=1)
contact_obj_frame_dict[inactive_arm] = None
contact_world_frame_dict[inactive_arm] = None
nearest_pt_world_dict[inactive_arm] = None
corner_norms[inactive_arm] = None
down_pcd_norms[inactive_arm] = None
elif primitive_name == 'grasp':
for key in contact_obj_frame.keys():
contact_obj_frame_dict[key] = util.pose_stamped2list(
contact_obj_frame[key])
contact_world_frame_dict[key] = util.pose_stamped2list(
contact_world_frame[key])
contact_pos = np.array(
contact_world_frame_dict[key][:3])
corner_dists = (np.asarray(keypoints_start) -
contact_pos)
corner_norms[key] = np.linalg.norm(corner_dists,
axis=1)
down_pcd_dists = (obs['down_pcd_pts'] - contact_pos)
down_pcd_norms[key] = np.linalg.norm(down_pcd_dists,
axis=1)
# contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[key][:3]))
# kdtree = open3d.geometry.KDTreeFlann(pcd)
# nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0]
# nearest_pt_world_dict[key] = np.asarray(pcd.points)[nearest_pt_ind]
print('Worker ID: %d, Waiting to plan...' % worker_id)
while True:
time.sleep(0.01)
work_queue_empty = work_queue.empty()
if work_queue_empty:
planner_available = True
break
if planner_available:
print('Worker ID: %d, Planning!...' % worker_id)
work_queue.put(True)
time.sleep(1.0)
result = action_planner.execute(primitive_name, plan_args)
time.sleep(1.0)
while not work_queue.empty():
work_queue.get()
if result is not None:
if result[0]:
success = True
global_info_dict['success'] += 1
sample = {}
sample['obs'] = obs
sample['start'] = start
sample['goal'] = goal
sample['keypoints_start'] = keypoints_start
sample['keypoints_goal'] = keypoints_goal
sample['keypoint_dists'] = corner_norms
sample['down_pcd_pts'] = obs['down_pcd_pts']
sample['down_pcd_dists'] = down_pcd_norms
sample['transformation'] = util.pose_from_matrix(T_mat)
sample['contact_obj_frame'] = contact_obj_frame_dict
sample[
'contact_world_frame'] = contact_world_frame_dict
# sample['contact_pcd'] = nearest_pt_world_dict
sample['result'] = result
if primitive_name == 'grasp':
sample['goal_face'] = exp_double.goal_face
if args.save_data:
data_manager.save_observation(
sample, str(global_info_dict['trial']))
# print("Success: " + str(global_info_dict['success']))
except ValueError as e:
print("Value error: ")
print(e)
result = None
time.sleep(1.0)
while not work_queue.empty():
work_queue.get()
worker_result = {}
worker_result['result'] = result
worker_result['id'] = worker_id
worker_result['success'] = success
result_queue.put(worker_result)
child_conn.send('DONE')
continue
if msg == "END":
break
time.sleep(0.001)
print('Breaking Worker ID: ' + str(worker_id))
child_conn.close()
def _add_constraint(self):
# collisions between robot_1 and robot_2
for i in range(len(self.linkNameToID_1)):
for j in range(len(self.linkNameToID_2)):
p.setCollisionFilterPair(self.robot_1, self.robot_2, i, j, 0)
# collisions between robot_1 and robot_3
for i in range(len(self.linkNameToID_1)):
for j in range(len(self.linkNameToID_2)):
p.setCollisionFilterPair(self.robot_1, self.robot_3, i, j, 0)
constraintNum = 20
# constraint between robot_1 and robot_2
for i in range(constraintNum):
p.createConstraint(parentBodyUniqueId=self.robot_1,
parentLinkIndex=self.linkNameToID_1["L_L_L_1"],
childBodyUniqueId=self.robot_2,
childLinkIndex=self.linkNameToID_2['L_L_2'],
jointType=p.JOINT_POINT2POINT,
jointAxis=[0, 0, 0],
parentFramePosition=[
-0.005 + (0.01 / constraintNum) * i, -0.06,
0
],
childFramePosition=[
-0.00722,
-0.005 + (0.01 / constraintNum) * i, 0.05211
])
for i in range(constraintNum):
p.createConstraint(parentBodyUniqueId=self.robot_1,
parentLinkIndex=self.linkNameToID_1["L_L_L_3"],
childBodyUniqueId=self.robot_2,
childLinkIndex=self.linkNameToID_2["L_L_2"],
jointType=p.JOINT_POINT2POINT,
jointAxis=[0, 0, 0],
parentFramePosition=[
-0.005 + (0.01 / constraintNum) * i, -0.055,
0
],
childFramePosition=[
0.04278,
-0.005 + (0.01 / constraintNum) * i,
0.102113
])
for i in range(constraintNum):
p.createConstraint(parentBodyUniqueId=self.robot_1,
parentLinkIndex=self.linkNameToID_1["L_R_L_1"],
childBodyUniqueId=self.robot_2,
childLinkIndex=self.linkNameToID_2['L_R_2'],
jointType=p.JOINT_POINT2POINT,
jointAxis=[0, 0, 0],
parentFramePosition=[
-0.005 + (0.01 / constraintNum) * i, -0.06,
0
],
childFramePosition=[
-0.005 + (0.01 / constraintNum) * i,
0.05211, -0.00722
])
for i in range(constraintNum):
p.createConstraint(parentBodyUniqueId=self.robot_1,
parentLinkIndex=self.linkNameToID_1["L_R_L_3"],
childBodyUniqueId=self.robot_2,
childLinkIndex=self.linkNameToID_2["L_R_2"],
jointType=p.JOINT_POINT2POINT,
jointAxis=[0, 0, 0],
parentFramePosition=[
-0.005 + (0.01 / constraintNum) * i, -0.055,
0
],
childFramePosition=[
-0.005 + (0.01 / constraintNum) * i,
0.102113, 0.04278
])
# constraint between robot_1 and robot_3
for i in range(constraintNum):
p.createConstraint(parentBodyUniqueId=self.robot_1,
parentLinkIndex=self.linkNameToID_1["R_L_L_1"],
childBodyUniqueId=self.robot_3,
childLinkIndex=self.linkNameToID_3['L_L_2'],
jointType=p.JOINT_POINT2POINT,
jointAxis=[0, 0, 0],
parentFramePosition=[
-0.005 + (0.01 / constraintNum) * i, -0.06,
0
],
childFramePosition=[
-0.00722,
-0.005 + (0.01 / constraintNum) * i, 0.05211
])
for i in range(constraintNum):
p.createConstraint(parentBodyUniqueId=self.robot_1,
parentLinkIndex=self.linkNameToID_1["R_L_L_3"],
childBodyUniqueId=self.robot_3,
childLinkIndex=self.linkNameToID_3["L_L_2"],
jointType=p.JOINT_POINT2POINT,
jointAxis=[0, 0, 0],
parentFramePosition=[
-0.005 + (0.01 / constraintNum) * i, -0.055,
0
],
childFramePosition=[
0.04278,
-0.005 + (0.01 / constraintNum) * i,
0.102113
])
for i in range(constraintNum):
p.createConstraint(parentBodyUniqueId=self.robot_1,
parentLinkIndex=self.linkNameToID_1["R_R_L_1"],
childBodyUniqueId=self.robot_3,
childLinkIndex=self.linkNameToID_3['L_R_2'],
jointType=p.JOINT_POINT2POINT,
jointAxis=[0, 0, 0],
parentFramePosition=[
-0.005 + (0.01 / constraintNum) * i, -0.06,
0
],
childFramePosition=[
-0.005 + (0.01 / constraintNum) * i,
0.05211, -0.00722
])
for i in range(constraintNum):
p.createConstraint(parentBodyUniqueId=self.robot_1,
parentLinkIndex=self.linkNameToID_1["R_R_L_3"],
childBodyUniqueId=self.robot_3,
childLinkIndex=self.linkNameToID_3["L_R_2"],
jointType=p.JOINT_POINT2POINT,
jointAxis=[0, 0, 0],
parentFramePosition=[
-0.005 + (0.01 / constraintNum) * i, -0.055,
0
],
childFramePosition=[
-0.005 + (0.01 / constraintNum) * i,
0.102113, 0.042785
])
def init_tool(self,
robot,
mesh_scale=[1] * 3,
pos_offset=[0] * 3,
orient_offset=[0, 0, 0, 1],
left=True,
maximal=False,
alpha=1.0):
if left:
gripper_pos, gripper_orient = p.getLinkState(
robot,
8,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
else:
gripper_pos, gripper_orient = p.getLinkState(
robot,
8,
computeForwardKinematics=True,
physicsClientId=self.id)[:2]
transform_pos, transform_orient = p.multiplyTransforms(
positionA=gripper_pos,
orientationA=gripper_orient,
positionB=pos_offset,
orientationB=orient_offset,
physicsClientId=self.id)
self.task = 'scratch_itch' # added by Pierre to choose the tool
if self.task == 'scratch_itch':
tool = p.loadURDF(os.path.join(self.directory, 'tools',
'tool_scratch.urdf'),
basePosition=transform_pos,
baseOrientation=transform_orient,
physicsClientId=self.id)
elif self.task == 'bed_bathing':
tool = p.loadURDF(os.path.join(self.directory, 'tools',
'wiper.urdf'),
basePosition=transform_pos,
baseOrientation=transform_orient,
physicsClientId=self.id)
elif self.task in [
'drinking', 'scooping', 'feeding', 'arm_manipulation'
]:
if self.task == 'drinking':
visual_filename = os.path.join(self.directory, 'tools',
'plastic_coffee_cup.obj')
collision_filename = os.path.join(
self.directory, 'tools', 'plastic_coffee_cup_vhacd.obj')
elif self.task in ['scooping', 'feeding']:
visual_filename = os.path.join(self.directory, 'tools',
'spoon_reduced_compressed.obj')
collision_filename = os.path.join(self.directory, 'tools',
'spoon_vhacd.obj')
elif self.task == 'arm_manipulation':
visual_filename = os.path.join(self.directory, 'tools',
'arm_manipulation_scooper.obj')
collision_filename = os.path.join(
self.directory, 'tools',
'arm_manipulation_scooper_vhacd.obj')
tool_visual = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName=visual_filename,
meshScale=mesh_scale,
rgbaColor=[1, 1, 1, alpha],
physicsClientId=self.id)
tool_collision = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=collision_filename,
meshScale=mesh_scale,
physicsClientId=self.id)
tool = p.createMultiBody(baseMass=0.01,
baseCollisionShapeIndex=tool_collision,
baseVisualShapeIndex=tool_visual,
basePosition=transform_pos,
baseOrientation=transform_orient,
useMaximalCoordinates=maximal,
physicsClientId=self.id)
if left:
# Disable collisions between the tool and robot
for j in [7, 8, 9, 10, 11, 12, 13, 14]:
for tj in list(
range(p.getNumJoints(tool,
physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot,
tool,
j,
tj,
False,
physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(
robot,
8,
tool,
-1,
p.JOINT_FIXED, [0, 0, 0],
parentFramePosition=pos_offset,
childFramePosition=[0, 0, 0],
parentFrameOrientation=orient_offset,
physicsClientId=self.id)
else:
# Disable collisions between the tool and robot
for j in [7, 8, 9, 10, 11, 12, 13, 14]:
for tj in list(
range(p.getNumJoints(tool,
physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot,
tool,
j,
tj,
False,
physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(
robot,
8,
tool,
-1,
p.JOINT_FIXED, [0, 0, 0],
parentFramePosition=pos_offset,
childFramePosition=[0, 0, 0],
parentFrameOrientation=orient_offset,
physicsClientId=self.id)
p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
return tool
# p.changeDynamics(pyramid, -1, lateralFriction=0.5)
# p.changeDynamics(pyramid, -1, restitution=0.5)
# p.changeDynamics(pyramid, -1, mass=0.5)
p.changeDynamics(wall, -1, lateralFriction=0.3)
p.changeDynamics(wall, -1, restitution=0.5)
# p.changeDynamics(wall, -1)
p.changeDynamics(wall_1, -1, lateralFriction=0.4)
p.changeDynamics(wall_1, -1, restitution=0.6)
# p.changeDynamics(wall, -1)
# p.setCollisionFilterGroupMask(pyramid, -1, 0,0 )
enableCol = 1
p.setCollisionFilterPair(obj, wall, -1, -1, enableCol)
p.setCollisionFilterPair(obj, wall_1, -1, -1, enableCol)
# p.setRealTimeSimulation(1)
f = 1
for i in range(3000):
pos, orn = p.getBasePositionAndOrientation(obj)
print(
'degree: ',
[180 * element / math.pi for element in p.getEulerFromQuaternion(orn)])
# pos1, orn1 = p.getBasePositionAndOrientation(wall)
if f:
time.sleep(3)
for j in range(5):
p.applyExternalForce(obj,
def collision_filter():
# disable the collision with left and right rim Because of the inproper collision shape
for id_f in JOINT_ID_F:
p.setCollisionFilterPair(iiwa, table, id_f, 0, 0)
p.setCollisionFilterPair(iiwa, table, id_f, 1, 0)
p.setCollisionFilterPair(iiwa, table, id_f, 2, 0)
p.setCollisionFilterPair(iiwa, table, id_f, 4, 0)
p.setCollisionFilterPair(iiwa, table, id_f, 5, 0)
for id_b in JOINT_ID_B:
p.setCollisionFilterPair(iiwa, table, id_b, 0, 0)
p.setCollisionFilterPair(iiwa, table, id_b, 1, 0)
p.setCollisionFilterPair(iiwa, table, id_b, 2, 0)
p.setCollisionFilterPair(iiwa, table, id_b, 4, 0)
p.setCollisionFilterPair(iiwa, table, id_b, 5, 0)
def collision_filter(self, puck, table):
p.setCollisionFilterPair(puck, table, 0, 0, 1)
p.setCollisionFilterPair(puck, table, 0, 1, 1)
p.setCollisionFilterPair(puck, table, 0, 2, 1)
p.setCollisionFilterPair(puck, table, 0, 4, 1)
p.setCollisionFilterPair(puck, table, 0, 5, 1)
p.setCollisionFilterPair(puck, table, 0, 6, 1)
p.setCollisionFilterPair(puck, table, 0, 7, 1)
p.setCollisionFilterPair(puck, table, 0, 8, 1)
def check_stability(input_file, gui=False):
# First, check if the file is even rooted.
# If it's not rooted, it can't be stable
if not check_rooted(input_file):
return False
# Start up the simulation
mode = p.GUI if gui else p.DIRECT
physicsClient = p.connect(mode)
p.setGravity(0, 0, -9.8)
# Load the ground plane
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# print(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
# Convert the object to a URDF assembly, load it up
# There may be more than one URDF, if the object had more than one connected component
obj2urdf(input_file, 'tmp')
objIds = []
startPositions = {}
scales = {}
for urdf in [
f for f in os.listdir('tmp') if os.path.splitext(f)[1] == '.urdf'
]:
with open(f'tmp/{os.path.splitext(urdf)[0]}.json', 'r') as f:
data = json.loads(f.read())
startPos = data['start_pos']
startOrientation = p.getQuaternionFromEuler([0, 0, 0])
objId = p.loadURDF(f"tmp/{urdf}", startPos, startOrientation)
objIds.append(objId)
startPositions[objId] = startPos
scales[objId] = data['scale']
shutil.rmtree('tmp')
# Disable collisions between all objects (we only want collisions between objects and the ground)
# That's because we want to check if the different components are *independently* stable, and
# having them hit each other might muck up that judgment
for i in range(0, len(objIds) - 1):
ni = p.getNumJoints(objIds[i])
for j in range(i + 1, len(objIds)):
nj = p.getNumJoints(objIds[j])
for k in range(-1, ni):
for l in range(-1, nj):
p.setCollisionFilterPair(objIds[i], objIds[j], k, l, False)
# See if objects are stable under some perturbation
for objId in objIds:
s = scales[objId]
p.applyExternalForce(objId, -1, (0, 0, 600 * s), startPositions[objId],
p.WORLD_FRAME)
p.applyExternalTorque(objId, -1, (0, 5 * s, 0), p.WORLD_FRAME)
p.applyExternalTorque(objId, -1, (5 * s, 0, 0), p.WORLD_FRAME)
p.applyExternalTorque(objId, -1, (0, 0, 200 * s), p.WORLD_FRAME)
# Run simulation
if gui:
while True:
p.stepSimulation()
time.sleep(1. / 240.)
else:
for i in range(10000):
p.stepSimulation()
for objId in objIds:
endPos, _ = p.getBasePositionAndOrientation(objId)
zend = endPos[2]
zstart = startPositions[objId][2]
zdiff = abs(zend - zstart)
if zdiff > abs(0.05 * scales[objId]):
p.disconnect()
return False
p.disconnect()
return True
def load(self):
ids = super(Fetch, self).load()
robot_id = self.robot_ids[0]
# disable collision between torso_lift_joint and shoulder_lift_joint
# between torso_lift_joint and torso_fixed_joint
# between caster_wheel_joint and estop_joint
# between caster_wheel_joint and laser_joint
# between caster_wheel_joint and torso_fixed_joint
# between caster_wheel_joint and l_wheel_joint
# between caster_wheel_joint and r_wheel_joint
p.setCollisionFilterPair(robot_id, robot_id, 3, 13, 0)
p.setCollisionFilterPair(robot_id, robot_id, 3, 22, 0)
p.setCollisionFilterPair(robot_id, robot_id, 0, 20, 0)
p.setCollisionFilterPair(robot_id, robot_id, 0, 21, 0)
p.setCollisionFilterPair(robot_id, robot_id, 0, 22, 0)
p.setCollisionFilterPair(robot_id, robot_id, 0, 1, 0)
p.setCollisionFilterPair(robot_id, robot_id, 0, 2, 0)
return ids
enableCollision = 0
# p.setJointMotorControlMultiDof(robot, LeftArm, p.POSITION_CONTROL, [0], targetVelocity=[0, 0, 0], positionGain=0,
# velocityGain=1, force=[1000, 1, 1])
#
#
def disable_parent_collision():
for num in range(p.getNumJoints(robot)):
info = p.getJointInfo(robot, num)
if info[16] != -1:
p.setCollisionFilterPair(robot, robot, info[0], info[16], 0)
disable_parent_collision()
p.setCollisionFilterPair(robot, robot, Spine2, LeftArm, 0)
p.setCollisionFilterPair(robot, robot, Spine2, RightArm, 0)
for j in range(p.getNumJoints(robot)):
ji = p.getJointInfo(robot, j)
targetPosition = [0]
jointType = ji[2]
if jointType == p.JOINT_SPHERICAL:
targetPosition = [0, 0, 0, 1]
# p.setJointMotorControlMultiDof(robot,
# j,
# p.POSITION_CONTROL,
# targetPosition,
# targetVelocity=[0, 0, 0],
# positionGain=0,
# velocityGain=1,
#enable collision between lower legs
for j in range(p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped, j))
#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1 > l0):
enableCollision = 1
print("collision for pair", l0, l1,
p.getJointInfo(quadruped, l0)[12],
p.getJointInfo(quadruped, l1)[12], "enabled=",
enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2, 5,
enableCollision)
jointIds = []
paramIds = []
jointOffsets = []
jointDirections = [-1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1]
jointAngles = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce", 0, 100, 20)
for j in range(p.getNumJoints(quadruped)):
def disable_parent_collision():
for num in range(p.getNumJoints(robot)):
info = p.getJointInfo(robot, num)
if info[16] != -1:
p.setCollisionFilterPair(robot, robot, info[0], info[16], 0)
if len(pred_poses) == 0:
print("No suitable grasp found.")
no_solution_fail += 1
else:
best_grasp = pred_poses[0][1] # (3, 4)
print("Confidence: %.4f" % pred_poses[0][0])
rotation = best_grasp[:3, :3]
trans_backward = best_grasp[:, 3]
approach = best_grasp[:3, 0]
trans = trans_backward + approach * deepen_hand
pose = np.append(rotation, trans[..., np.newaxis], axis=1)
pose_backward = np.append(rotation,
trans_backward[..., np.newaxis],
axis=1)
for link_name, link_id in tm_link_name_to_index.items():
p.setCollisionFilterPair(test._tm700.tm700Uid,
test.tableUid, link_id, -1, 0)
for obj_id, obj in obj_link_name_to_index:
for obj_name, obj_link in obj.items():
# temporary disable collision detection and move to ready pose
p.setCollisionFilterPair(
test._tm700.tm700Uid, obj_id, link_id,
obj_link, 0)
# Ready to grasp pose
test.step_to_target_pose([pose_backward, -0.0],
ts=ts,
max_iteration=5000,
min_iteration=1)
# Enable collision detection to test if a grasp is successful.
for link_name, link_id in tm_link_name_to_index.items():
for obj_id, obj in obj_link_name_to_index:
for obj_name, obj_link in obj.items():
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class, loads the robot into
the simulated instance. This method also updates the max velocity of
the robot's fingers, adds self collision filters to the model and
defines the cameras of the model in the camera_dict.
Parameters:
translation - List containing 3 elements, the translation [x, y, z]
of the robot in the WORLD frame
quaternion - List containing 4 elements, the quaternion
[x, y, z, q] of the robot in the WORLD frame
physicsClientId - The id of the simulated instance in which the
robot is supposed to be loaded
Returns:
boolean - True if the method ran correctly, False otherwise
"""
pybullet.setAdditionalSearchPath(
os.path.dirname(os.path.realpath(__file__)),
physicsClientId=physicsClientId)
# Add 0.36 meters on the z component, avoing to spawn NAO in the ground
translation = [translation[0], translation[1], translation[2] + 0.36]
RobotVirtual.loadRobot(
self,
translation,
quaternion,
physicsClientId=physicsClientId)
balance_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=translation,
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.goToPosture("Stand", 1.0)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict["Head"].getIndex(),
0,
physicsClientId=self.physics_client)
for side in ["R", "L"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Thigh"].getIndex(),
self.link_dict[side + "Hip"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Bicep"].getIndex(),
self.link_dict[side + "ForeArm"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Pelvis"].getIndex(),
self.link_dict[side + "Thigh"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Tibia"].getIndex(),
self.link_dict[side.lower() + "_ankle"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Finger11_link"].getIndex(),
self.link_dict[side + "Finger13_link"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Finger21_link"].getIndex(),
self.link_dict[side + "Finger23_link"].getIndex(),
0,
physicsClientId=self.physics_client)
for base_link in ["RThigh", "LThigh", "RBicep", "LBicep"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[base_link].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
for wrist in ["r_wrist", "l_wrist"]:
if wrist[0] + "finger" in name.lower() or\
wrist[0] + "thumb" in name.lower():
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[wrist].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
for joint in self.joint_dict.values():
pybullet.resetJointState(
self.robot_model,
joint.getIndex(),
0.0)
pybullet.removeConstraint(
balance_constraint,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
camera_top = CameraRgb(
self.robot_model,
NaoVirtual.ID_CAMERA_TOP,
self.link_dict["CameraTop_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
camera_bottom = CameraRgb(
self.robot_model,
NaoVirtual.ID_CAMERA_BOTTOM,
self.link_dict["CameraBottom_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
self.camera_dict = {
NaoVirtual.ID_CAMERA_TOP: camera_top,
NaoVirtual.ID_CAMERA_BOTTOM: camera_bottom}
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class, loads the robot into
the simulated instance. This method also updates the max velocity of
the robot's fingers, adds self collision filters to the model, adds a
motion constraint to the model and defines the cameras of the model in
the camera_dict.
Parameters:
translation - List containing 3 elements, the translation [x, y, z]
of the robot in the WORLD frame
quaternion - List containing 4 elements, the quaternion
[x, y, z, q] of the robot in the WORLD frame
physicsClientId - The id of the simulated instance in which the
robot is supposed to be loaded
Returns:
boolean - True if the method ran correctly, False otherwise
"""
pybullet.setAdditionalSearchPath(os.path.dirname(
os.path.realpath(__file__)),
physicsClientId=physicsClientId)
RobotVirtual.loadRobot(self,
translation,
quaternion,
physicsClientId=physicsClientId)
for link in ["Hip", "Pelvis", "Head"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[link].getIndex(),
0,
physicsClientId=self.physics_client)
for shoulder_roll_link in ["RBicep", "LBicep"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[shoulder_roll_link].getIndex(),
0,
physicsClientId=self.physics_client)
for side in ["R", "L"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "ForeArm"].getIndex(),
self.link_dict[side + "Bicep"].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
for wrist in ["r_wrist", "l_wrist"]:
if wrist[0] + "finger" in name.lower() or\
wrist[0] + "thumb" in name.lower():
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[wrist].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
elif "Wheel" in joint_name:
self.joint_dict.pop(joint_name)
camera_top = CameraRgb(self.robot_model,
PepperVirtual.ID_CAMERA_TOP,
self.link_dict["CameraTop_optical_frame"],
hfov=56.3,
vfov=43.7,
physicsClientId=self.physics_client)
camera_bottom = CameraRgb(self.robot_model,
PepperVirtual.ID_CAMERA_BOTTOM,
self.link_dict["CameraBottom_optical_frame"],
hfov=56.3,
vfov=43.7,
physicsClientId=self.physics_client)
camera_depth = CameraDepth(self.robot_model,
PepperVirtual.ID_CAMERA_DEPTH,
self.link_dict["CameraDepth_optical_frame"],
hfov=58.0,
vfov=45.0,
physicsClientId=self.physics_client)
self.camera_dict = {
PepperVirtual.ID_CAMERA_TOP: camera_top,
PepperVirtual.ID_CAMERA_BOTTOM: camera_bottom,
PepperVirtual.ID_CAMERA_DEPTH: camera_depth
}
self.motion_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=[translation[0], translation[1], 0],
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.laser_manager = Laser(self.robot_model,
self.link_dict["Tibia"].getIndex(),
physicsClientId=self.physics_client)
self.base_controller = PepperBaseController(
self.robot_model, [self.linear_velocity, self.angular_velocity],
[self.linear_acceleration, self.angular_acceleration],
self.motion_constraint,
physicsClientId=self.physics_client)
self.goToPosture("StandZero", 1.0)
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class. Update max velocity
for the fingers and thumbs, based on the hand joints. Add self
collision exceptions (The biceps won't autocollide with the torso, the
fingers and thumbs of a hand won't autocollide with the corresponding
wrist). Add the cameras. Add motion constraint.
"""
pybullet.setAdditionalSearchPath(
os.path.dirname(os.path.realpath(__file__)),
physicsClientId=physicsClientId)
RobotVirtual.loadRobot(
self,
translation,
quaternion,
physicsClientId=physicsClientId)
for base_link in ["Hip", "Pelvis"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[base_link].getIndex(),
0,
physicsClientId=self.physics_client)
for shoulder_roll_link in ["RBicep", "LBicep"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[shoulder_roll_link].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
for wrist in ["r_wrist", "l_wrist"]:
if wrist[0] + "finger" in name.lower() or\
wrist[0] + "thumb" in name.lower():
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[wrist].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
elif "Wheel" in joint_name:
self.joint_dict.pop(joint_name)
self.camera_top = CameraRgb(
self.robot_model,
self.link_dict["CameraTop_optical_frame"],
hfov=56.3,
vfov=43.7,
physicsClientId=self.physics_client)
self.camera_bottom = CameraRgb(
self.robot_model,
self.link_dict["CameraBottom_optical_frame"],
hfov=56.3,
vfov=43.7,
physicsClientId=self.physics_client)
self.camera_depth = CameraDepth(
self.robot_model,
self.link_dict["CameraDepth_optical_frame"],
hfov=58.0,
vfov=45.0,
physicsClientId=self.physics_client)
self.motion_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=[translation[0], translation[1], 0],
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.laser_manager = Laser(
self.robot_model,
self.link_dict["Tibia"].getIndex(),
physicsClientId=self.physics_client)
self.base_controller = PepperBaseController(
self.robot_model,
[self.linear_velocity, self.angular_velocity],
[self.linear_acceleration, self.angular_acceleration],
self.motion_constraint,
physicsClientId=self.physics_client)
self.goToPosture("StandZero", 1.0)
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf",[0,0,.5],[0,0.5,0.5,0], flags = urdfFlags,useFixedBase=False)
#enable collision between lower legs
for j in range (p.getNumJoints(quadruped)):
print(p.getJointInfo(quadruped,j))
#2,5,8 and 11 are the lower legs
lower_legs = [2,5,8,11]
for l0 in lower_legs:
for l1 in lower_legs:
if (l1>l0):
enableCollision = 1
print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
jointIds=[]
paramIds=[]
jointOffsets=[]
jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1]
jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0]
for i in range (4):
jointOffsets.append(0)
jointOffsets.append(-0.7)
jointOffsets.append(0.7)
maxForceId = p.addUserDebugParameter("maxForce",0,100,20)
for j in range (p.getNumJoints(quadruped)):
model_urdf_directory_extension = "cs292c_robot_models/octopus_generated_" + str(number_of_links_urdf) + "_links.urdf"
# this line loads a URDF (3D robot model) into pybullet physics simulator, and it returns a unique ID. That unique ID is needed when querying information about the robot. It is also used when controlling the robot
# octopusBodyUniqueId = p.loadURDF( fileName=os.path.join(pybullet_data.getDataPath(), model_urdf_directory_extension), flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
# print(os.getcwd())
# p.setAdditionalSearchPath(os.getcwd())
pendulum_uniqueId_pybullet = p.loadURDF(
fileName=os.path.join(pybullet_data.getDataPath(), model_urdf_directory_extension),
flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
pendulum_uniqueId_z3 = p.loadURDF(fileName=os.path.join(pybullet_data.getDataPath(), model_urdf_directory_extension),
flags=p.URDF_USE_SELF_COLLISION | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
p.setCollisionFilterPair(bodyUniqueIdA=pendulum_uniqueId_pybullet,
bodyUniqueIdB=pendulum_uniqueId_z3,
linkIndexA=0,
linkIndexB=0,
enableCollision=0
)
p.setCollisionFilterPair(bodyUniqueIdA=pendulum_uniqueId_pybullet,
bodyUniqueIdB=pendulum_uniqueId_z3,
linkIndexA=0,
linkIndexB=-1,
enableCollision=0
)
p.setCollisionFilterPair(bodyUniqueIdA=pendulum_uniqueId_pybullet,
bodyUniqueIdB=pendulum_uniqueId_z3,
linkIndexA=-1,
linkIndexB=0,
enableCollision=0
)
