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
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 )