def check_collision(self, other_id=None, collision_distance=0.0): others_id = [other_id] if other_id is None: others_id = [ p.getBodyUniqueId(i) for i in range(p.getNumBodies()) if p.getBodyUniqueId(i) != self.id ] for other_id in others_id: if len( p.getClosestPoints(bodyA=self.id, bodyB=other_id, distance=collision_distance)) != 0: return True return False
def run(self) -> Dict[core.PhysicalObject, Dict[str, list]]: steps_per_frame = self.scene.step_rate // self.scene.frame_rate max_step = (self.scene.frame_end + 1) * steps_per_frame obj_idxs = [pb.getBodyUniqueId(i) for i in range(pb.getNumBodies())] animation = { obj_id: { "position": [], "quaternion": [], "velocity": [], "angular_velocity": [] } for obj_id in obj_idxs } for current_step in range(max_step): if current_step % steps_per_frame == 0: for obj_idx in obj_idxs: position, quaternion = self.get_position_and_rotation( obj_idx) velocity, angular_velocity = self.get_velocities(obj_idx) animation[obj_idx]["position"].append(position) animation[obj_idx]["quaternion"].append(quaternion) animation[obj_idx]["velocity"].append(velocity) animation[obj_idx]["angular_velocity"].append( angular_velocity) pb.stepSimulation() return { self.objects_to_pybullet.inverse[obj_idx]: anim for obj_idx, anim in animation.items() }
def createMaiskolben(x, y, z, m_list, m_hit): for i in range(10): h = i * 0.4 + z pixel_1 = p.loadURDF("pixel.urdf", [0.52263 + x, 0 + y, 3 + h], useMaximalCoordinates=False) pixel_2 = p.loadURDF("pixel.urdf", [-0.52263 + x, 0 + y, 3 + h], useMaximalCoordinates=False) pixel_3 = p.loadURDF("pixel.urdf", [0 + x, 0.52263 + y, 3 + h], useMaximalCoordinates=False) pixel_4 = p.loadURDF("pixel.urdf", [0 + x, -0.52263 + y, 3 + h], useMaximalCoordinates=False) pixel_5 = p.loadURDF("pixel.urdf", [0.36995 + x, 0.36995 + y, 3 + h], useMaximalCoordinates=False) pixel_6 = p.loadURDF("pixel.urdf", [-0.36995 + x, -0.36995 + y, 3 + h], useMaximalCoordinates=False) pixel_7 = p.loadURDF("pixel.urdf", [0.36995 + x, -0.36995 + y, 3 + h], useMaximalCoordinates=False) pixel_8 = p.loadURDF("pixel.urdf", [-0.36995 + x, 0.36995 + y, 3 + h], useMaximalCoordinates=False) m_list.append(pixel_1) m_list.append(pixel_2) m_list.append(pixel_3) m_list.append(pixel_4) m_list.append(pixel_5) m_list.append(pixel_6) m_list.append(pixel_7) m_list.append(pixel_8) j = 0 for pixel in m_list: body = p.getBodyUniqueId(pixel) while len(m_hit) < body: m_hit.append(j) m_hit.append(j)
def update_closest_points(self): others_id = [ p.getBodyUniqueId(i) for i in range(p.getNumBodies()) if p.getBodyUniqueId(i) != self.body_id ] self.closest_points_to_others = [ sorted(list( p.getClosestPoints(bodyA=self.body_id, bodyB=other_id, distance=self.max_distance_from_others)), key=lambda contact_points: contact_points[8]) if other_id != 0 else [] for other_id in others_id ] self.closest_points_to_self = [ p.getClosestPoints(bodyA=self.body_id, bodyB=self.body_id, distance=0, linkIndexA=link1, linkIndexB=link2) for link1, link2 in self.link_pairs ]
def check_overlap(self, obj: core.PhysicalObject) -> bool: obj_idx = self.objects_to_pybullet[obj] body_ids = [pb.getBodyUniqueId(i) for i in range(pb.getNumBodies())] for body_id in body_ids: if body_id == obj_idx: continue overlap_points = pb.getClosestPoints(obj_idx, body_id, distance=0) if overlap_points: # TODO: we can easily get a suggested correction here # i = np.argmin([o[8] for o in overlap_points], axis=0) # find the most overlapping point # push = np.array(overlap_points[i][7]) * (overlap_points[i][8] + margin) return True return False
def test(args): count = 0 env = gym.make(args.env) env.env.configure(args) print("args.render=", args.render) if (args.render == 1): env.render(mode="human") env.reset() w, h, vmat,projmat,camup,camfwd,hor,ver,yaw,pitch,dist,target= p.getDebugVisualizerCamera() dist = 0.4 yaw = 0 p.resetDebugVisualizerCamera(dist,yaw, pitch,target) for obindex in range (p.getNumBodies()): obuid = p.getBodyUniqueId(obindex) p.changeDynamics(obuid, -1, linearDamping=0, angularDamping=0) for l in range (p.getNumJoints(obuid)): p.changeDynamics(obuid, l, linearDamping=0, angularDamping=0, jointDamping=0) #if (l==0): # p.setJointMotorControl2(obuid,l,p.POSITION_CONTROL,targetPosition=0) if (l==2): jp,jv,_,_ = p.getJointState(obuid,l) p.resetJointState(obuid,l, jp,0.01 ) if (args.resetbenchmark): while (1): env.reset() print("p.getNumBodies()=", p.getNumBodies()) print("count=", count) count += 1 print("action space:") sample = env.action_space.sample() action = sample * 0.0 action = [0,0]#sample * 0.0 print("action=") print(action) for i in range(args.steps): obs, rewards, done, _ = env.step(action) if (args.rgb): print(env.render(mode="rgb_array")) print("obs=") print(obs) print("rewards") print(rewards) print("done") print(done)
def run(self, duration: float = 1.0) -> Dict[Object3D, Dict[str, list]]: max_step = math.floor(self.step_rate * duration) steps_per_frame = self.step_rate // self.frame_rate obj_idxs = [pb.getBodyUniqueId(i) for i in range(pb.getNumBodies())] animation = {obj_id: {"position": [], "orient_quat": []} for obj_id in obj_idxs} for current_step in range(max_step): if current_step % steps_per_frame == 0: for obj_idx in obj_idxs: pos, quat = pb.getBasePositionAndOrientation(obj_idx) animation[obj_idx]["position"].append(pos) animation[obj_idx]["orient_quat"].append(quat) # roll, pitch, yaw pb.stepSimulation() return {self.objects_by_idx[obj_idx]: anim for obj_idx, anim in animation.items()}
def high_contrast_bodies(alpha: float = 0.5): """Makes all bodies transparent and gives each body an individual color. Args: alpha (float): Regulates the strength of transparency. """ num_bodies = p.getNumBodies() hsv = [(x * 1.0 / num_bodies, 0.5, 0.5) for x in range(num_bodies)] rgb = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv)) for i in range(num_bodies): body_unique_id = p.getBodyUniqueId(i) for link_index in range(-1, p.getNumJoints(body_unique_id)): p.changeVisualShape( body_unique_id, link_index, rgbaColor=[rgb[i][0], rgb[i][1], rgb[i][2], alpha])
def get_sensor_image(self): """ Returns the raw and clipped sensor image. The clipped image is generated based on the thickness of the tactile layer. Returns: (np.array, np.array, np.array, np.array) : RGB image, clipped RGB image, clipped depth image, seg image. """ # update sensor position and parameters self._update_pose() self._update_sensor() rgb_img, depth_img, seg_img = self._camera.get_pybullet_image() # update the contact information self._contacts = Contact(self._sensor_id) # clip the images to the depth of the tactile sensor mask = np.where(depth_img >= self.max_buffer_depth) depth_img[mask] = self.max_buffer_depth # basically all the RGB image should be the color of the tactile surface clipped_rgb_img = np.copy(rgb_img) clipped_rgb_img[:, :, :] = self.background_color # clip the segmentation image clipped_seg_img = np.copy(seg_img) clipped_seg_img[mask] = -1 if self._use_force: # store the raw clipped images in the image buffer with the Z position of the object obj_id = p.getBodyUniqueId(p.getNumBodies() - 1) position, _ = p.getBasePositionAndOrientation(obj_id) self._image_buf.store(clipped_rgb_img, depth_img, clipped_seg_img, position[-1], self._time) # compute the penetration based on the equilibrium of contact information and the spring models img_equilibrium = self.compute_equilibrium() return rgb_img, img_equilibrium['rgb_img'], img_equilibrium[ 'depth_img'], seg_img, img_equilibrium['seg_img'] else: return rgb_img, clipped_rgb_img, depth_img, seg_img, clipped_seg_img
def run_sim(): physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally p.setGravity(0, 0, -10) planeId = p.loadURDF("assets/plane.urdf") startPos = [0, 0, 1] startOrientation = p.getQuaternionFromEuler([0, 0, 0]) robot = p.loadURDF("assets/my-robot/robot.urdf", startPos, startOrientation) # print("------") wheel_rad = 0.0762 # print(p.getBodyUniqueId(robot)) bodyUniqueID = p.getBodyUniqueId(robot) # for i in range(0, p.getNumJoints(robot)): # print(p.getJointInfo(robot, i)) # p.setJointMotorControlArray(bodyUniqueID, [2, 3, 1, 0], p.VELOCITY_CONTROL, targetVelocities = [100, 100, 100, 100]) interface = OneWayWPILibWSInterfaceApp() # leftVel = 0 # rightVel = 0 timeStep = 0.015 p.setTimeStep(timeStep) while True: startTime = time.time() leftVel, rightVel = interface.get_velocity() leftVel = leftVel / wheel_rad rightVel = rightVel / wheel_rad # print(leftVel, rightVel) p.setJointMotorControlArray( bodyUniqueID, [2, 3, 1, 0], p.VELOCITY_CONTROL, targetVelocities=[leftVel, leftVel, rightVel, rightVel]) p.stepSimulation() print(time.time() - startTime) time.sleep(timeStep) # print(p.getJointStates(bodyUniqueID, [2, 3, 1, 0])) # for state in p.getJointStates(bodyUniqueID, [2, 3, 1, 0]): # print(state[1]) cubePos, cubeOrn = p.getBasePositionAndOrientation(robot) print(cubePos, cubeOrn) p.disconnect()
def getSceneAABB(): aabbMins = [] aabbMaxs = [] for i in range(p.getNumBodies()): uid = p.getBodyUniqueId(i) aabb = p.getAABB(uid) aabbMins.append(np.array(aabb[0])) aabbMaxs.append(np.array(aabb[1])) if len(aabbMins): sceneAABBMin = aabbMins[0] sceneAABBMax = aabbMaxs[0] for aabb in aabbMins: sceneAABBMin = np.minimum(sceneAABBMin, aabb) for aabb in aabbMaxs: sceneAABBMax = np.maximum(sceneAABBMax, aabb) print("sceneAABBMin=", sceneAABBMin) print("sceneAABBMax=", sceneAABBMax)
def get_body_ids(): return sorted([p.getBodyUniqueId(i) for i in range(p.getNumBodies())])
def get_bodies(): return [p.getBodyUniqueId(i) for i in range(p.getNumBodies())]
def __init__( self, xml_path='/home/shjliu/workspace/EPR/bullet3-new/test-bullet/roomdoor', num_solver_iterations=10, frame_skip=4, time_step=1 / 240., debug=0, render=0, actuator_lower_limit=(), actuator_upper_limit=(), robot_init_pos=(0, 0, 200), door_init_pos=(3, 0, 0), goal_pos=(3, 0, 0), door_controller_k=10**5, door_controller_d1=10**4, door_controller_d2=10**2, door_controller_th_handle=-30 * np.pi / 180, door_controller_th_door=5 * np.pi / 180, handle_controller_k=10**0.3, handle_controller_d=10**-1): # Set goal self.goal_pos = goal_pos self.robot_init_pos = robot_init_pos self.door_init_pos = door_init_pos # Camera parameters self._cam_dist = 5 self._cam_yaw = 60 self._cam_pitch = -66 self._render_width = 320 self._render_height = 240 # Open physics client, load files and set parameters self.physicsClient_ID = -1 self.xml_path = xml_path self.num_solver_iterations = num_solver_iterations self.frame_skip = frame_skip self.time_step = time_step self._set_physics_client(render) # Create relevant dictionaries body_name_2_joints = {} for body_id in range(pybullet.getNumBodies()): # Obtain name of specific body and decode it (bin_body_name, _) = pybullet.getBodyInfo(body_id) body_name = bin_body_name.decode("utf8") if body_name == "base_link": body_name = "door" # Get the amount of joints, their names and put them into a dictionary num_joints = pybullet.getNumJoints(body_id) joint_name_2_joint_id = {} if num_joints: for joint_id in range(num_joints): (_, bin_joint_name, joint_type, q_index, u_index, flags, joint_damping, joint_friction, joint_lower_limit, joint_upper_limit, joint_max_force, joint_max_velocity, link_name, joint_axis, parent_frame_pos, parent_frame_orn, parent_index) = pybullet.getJointInfo(body_id, joint_id) joint_name = bin_joint_name.decode("utf8") if joint_type == 4: num_joints -= 1 else: joint_name_2_joint_id.update({ joint_name: { "joint_id": joint_id, "joint_type": joint_type, "joint_damping": joint_damping, "joint_lower_limit": joint_lower_limit, "joint_upper_limit": joint_upper_limit } }) for i in joint_name_2_joint_id: print("joint info=", i, joint_name_2_joint_id[i]) body_name_2_joints.update({ body_name: { "body_id": body_id, "unique_body_id": pybullet.getBodyUniqueId(body_id), "body_name": body_name, "num_joints": num_joints, "joint_dict": joint_name_2_joint_id } }) self.body_name_2_joints = body_name_2_joints print("body info=", self.body_name_2_joints) # Important link ID's self.door_link_IDs = { 'handle_inside': 7, 'handle_outside': 9, 'door_COM': 5 } self.robot_link_IDs = { 'torso': 0, 'right_hand': 28, 'left_hand': 34, } # Indicate names of controllable joints: ASSUMES ROBOT ROOT BODY IS NAMED 'robot' AND DOOR ROOT BODY 'door' self.robot_joints = [ joint_name for joint_name in body_name_2_joints['robot']['joint_dict'] ] self.robot_joints_index = [ body_name_2_joints['robot']['joint_dict'][joint_name]['joint_id'] for joint_name in body_name_2_joints['robot']['joint_dict'] ] self.door_joints = [ joint_name for joint_name in body_name_2_joints['door']['joint_dict'] ] self.door_joints_index = [ body_name_2_joints['door']['joint_dict'][joint_name]['joint_id'] for joint_name in body_name_2_joints['door']['joint_dict'] ] # joint_low = [body_name_2_joints['robot']['joint_dict'][joint_name]['joint_lower_limit'] for joint_name in body_name_2_joints['robot']['joint_dict']] # joint_high = [body_name_2_joints['robot']['joint_dict'][joint_name]['joint_upper_limit'] for joint_name in body_name_2_joints['robot']['joint_dict']] # Initialize door controllers self.door_controller_k = door_controller_k self.door_controller_d1 = door_controller_d1 self.door_controller_d2 = door_controller_d2 self.door_controller_th_handle = door_controller_th_handle self.door_controller_th_door = door_controller_th_door self.handle_controller_k = handle_controller_k self.handle_controller_d = handle_controller_d # Initialize cost function weight placeholders. Will be set in hidden method self.w_alive, self.w_hand_on_handle, self.w_at_target, self.w_move_door, self.w_move_handle, self.w_time, self.w_control = ( 0, 0, 0, 0, 0, 0, 0) self.set_reward_weights() # Initialize action and observation space if not actuator_lower_limit: actuator_lower_limit = -np.pi * np.ones_like( self.robot_joints_index) actuator_upper_limit = np.pi * np.ones_like( self.robot_joints_index) else: assert (len(self.robot_joints_index) == len(actuator_lower_limit) & len(self.robot_joints_index) == len(actuator_upper_limit)) self.action_space = gym.spaces.Box(low=actuator_lower_limit, high=actuator_upper_limit, dtype=np.float32) self.action = self.action_space.sample() obs, _, _, _ = self.step(self.action, debug=0) low = np.full(obs.shape, -float('inf')) high = np.full(obs.shape, float('inf')) self.observation_space = gym.spaces.Box(low=low, high=high, dtype=obs.dtype) self.obs_dict = {} self.reward_dict = {} # Debug if debug: self._debug_link_ids = [] self._debug_lines_ids = [] self._debug_links_text_init() self._debug_links_text_remove() self._debug_links_lines_init() self._debug_links_lines_remove()
def main(): p.setRealTimeSimulation(1) p.setGravity(0, 0, 0) # we don't use gravity yet # debug colors only for show colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1], [0, 0, 0, 1] ] currentColor = 0 try: body = p.getBodyUniqueId(mantis_robot) EndEffectorIndex = p.getNumJoints(body) -1 print("EndEffectorIndex %i" % EndEffectorIndex) tx = 0 ty = 5 tz = 3 tj = 0 tp = 0 tr = math.pi / 2 orn = p.getQuaternionFromEuler([tj,tp,tr]) jointT = p.calculateInverseKinematics(body, EndEffectorIndex, [tx, ty,tz], orn, jointDamping=jd) p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT ) print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr)) print(jointT) p.resetBasePositionAndOrientation(target, [tx, ty, tz ], orn) loop = 0 hasPrevPose = 0 trailDuration = 15 targetSelected = False distance = 5 yaw = 90 motorDir = [1,-1,1,1,1,1] while (p.isConnected()): time.sleep(1. / 240.) # set to 240 fps p.stepSimulation() # get target positon targetPos, targetOrn = p.getBasePositionAndOrientation(target) # do Inverse Kinematics # jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, targetOrn) jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, orn) # set Robot Joints p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT ) # reduce the output speed to 1/10 of the simulation speed # ~24 fps loop +=1 if loop > 10: loop = 0 # sends data to the robot over ethernet/UDP # just a simple string with the motor number and the angle positon in 12 bit range (0-4096) data_string = "" for i in range ( 1, EndEffectorIndex): jt = p.getJointState(body, i ) data_string += "motor%i:%i\r\n" % ( i, scale(motorDir[i-1] * jt[0])) transfer_socket.sendto(bytes(data_string, "utf-8"), (transfer_ip, transfer_port)) # generates the trail for debug only # optional ls = p.getLinkState(body, EndEffectorIndex) targetPos, targetOrn = p.getBasePositionAndOrientation(target) if (hasPrevPose): p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1, trailDuration) p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration) prevPose = targetPos prevPose1 = ls[4] hasPrevPose = 1 # get mouse events # changes color of the object clickt on and prints some info mouseEvents = p.getMouseEvents() for e in mouseEvents: if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_RELEASED) and (targetSelected == True) ): # after mouse lost reset posion to prevent the target from floting around targetSelected = False targetPos, targetOrn = p.getBasePositionAndOrientation(target) p.resetBasePositionAndOrientation(target, targetPos, targetOrn) if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)): mouseX = e[1] mouseY = e[2] rayFrom, rayTo = getRayFromTo(mouseX, mouseY) rayInfo = p.rayTest(rayFrom, rayTo) for l in range(len(rayInfo)): hit = rayInfo[l] objectUid = hit[0] jointUid = hit[1] if( objectUid == target): targetSelected = True if (objectUid >= 1): # this is for debug click on an object to get id # oject will change color this has no effect print("obj %i joint %i" % (objectUid , jointUid)) p.changeVisualShape(objectUid, jointUid, rgbaColor=colors[currentColor]) currentColor += 1 if (currentColor >= len(colors)): currentColor = 0 except KeyboardInterrupt: print("KeyboardInterrupt has been caught.") finally: endProgramm()
useMaximalCoordinates=False) pixel_8 = p.loadURDF("pixel.urdf", [-0.36995, 0.36995, 3 + h], useMaximalCoordinates=False) maiskolben_1.append(pixel_1) maiskolben_1.append(pixel_2) maiskolben_1.append(pixel_3) maiskolben_1.append(pixel_4) maiskolben_1.append(pixel_5) maiskolben_1.append(pixel_6) maiskolben_1.append(pixel_7) maiskolben_1.append(pixel_8) j = 0 for pixel in maiskolben_1: body = p.getBodyUniqueId(pixel) while len(maiskolben_1_hit) < body: maiskolben_1_hit.append(j) maiskolben_1_hit.append(j) collisionFilterGroup = 0 collisionFilterMask = 0 p.setCollisionFilterGroupMask(col, -1, collisionFilterGroup, collisionFilterMask) p.setCollisionFilterGroupMask(col2, -1, collisionFilterGroup, collisionFilterMask) collisionFilterGroup = 0 collisionFilterMask = 0
def bodies(self): for i in range(p.getNumBodies()): bid = p.getBodyUniqueId(i) if bid != self.planeId and bid != self.gid and bid != self.bin: yield bid
exclude[1,5] = 1 exclude[1,8] = 0 for j in range(len(bodies)): for i in range(p.getNumJoints(bodies[j])): p.setJointMotorControl2(bodies[j],i,p.VELOCITY_CONTROL, force=0) _,name,_,_,_,_,_,_,minLimit,maxLimit,_,_,linkName = p.getJointInfo(bodies[j],i) print "Joint named ", name, "indices:",j, "," , i print "Link named" , linkName print "\n" print bodies for x in range(0,p.getNumBodies(0)): print "Body: (",x,")", p.getBodyInfo(p.getBodyUniqueId(x)) t = 0 while True: #print math.sin(t*speed)/2*(maxLimit-minLimit)+minLimit for j in range(len(bodies)): for i in range(p.getNumJoints(bodies[j])): _,name,_,_,_,_,_,_,minLimit,maxLimit,_,_,_ = p.getJointInfo(bodies[j],i) if exclude[j,i] == 0: p.setJointMotorControl2(bodies[j],i,p.POSITION_CONTROL,math.sin(t*speed)/2*(maxLimit-minLimit),p_gain) #print p.getJointState(bodies[j],i) p.stepSimulation() t = t + fixedTimeStep #for j in range(len(bodies)):
def get_body_names(): return [ p.getBodyInfo(p.getBodyUniqueId(i))[1] for i in range(p.getNumBodies()) ]
def main(): p.setRealTimeSimulation(1) p.setGravity(0, 0, 0) # we don't use gravity yet # debug colors only for show colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1], [0, 0, 0, 1]] currentColor = 0 try: print("open SpaceMouse") print(spacenavigator.list_devices()) success = spacenavigator.open() body = p.getBodyUniqueId(mantis_robot) EndEffectorIndex = p.getNumJoints(body) - 1 print("") print("EndEffectorIndex %i" % EndEffectorIndex) # i = 0 # for i in range(0, p.getNumJoints(body)): # n = p.getJointInfo(body,i) # alpha robot orientierung # orn = p.getQuaternionFromEuler([math.pi/2,0,math.pi/2]) # p.resetBasePositionAndOrientation(body, [0, 0, 2 ], orn) tx = 0 ty = 5 tz = 3 tj = 0 tp = 0 tr = math.pi / 2 orn = p.getQuaternionFromEuler([tj, tp, tr]) jointT = p.calculateInverseKinematics(body, EndEffectorIndex, [tx, ty, tz], orn, jointDamping=jd) p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6], controlMode=p.POSITION_CONTROL, targetPositions=jointT) print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx, ty, tz, tj, tp, tr)) print(jointT) p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn) loop = 0 hasPrevPose = 0 trailDuration = 15 distance = 5 yaw = 90 motorDir = [1, -1, 1, 1, 1, 1] while (p.isConnected()): time.sleep(1. / 240.) # set to 40 fps p.stepSimulation() # targetVelocity = p.readUserDebugParameter(targetVelocitySlider) if success: state = spacenavigator.read() # print(state) tx += state[1] * 0.01 ty += state[2] * 0.01 tz += state[3] * 0.01 # tj += state[4]*0.005 tp += state[5] * 0.005 tr += state[6] * -0.005 # print("x:%f y:%f z:%f j:%f p:%f r:%f " % (state[1],state[2],state[3],state[4],state[5],state[6])) # orn = p.getQuaternionFromEuler([tj,tp,tr]) orn = p.getQuaternionFromEuler([tj, tp, tr]) jointT = p.calculateInverseKinematics(body, EndEffectorIndex, [tx, ty, tz], orn) p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6], controlMode=p.POSITION_CONTROL, targetPositions=jointT) # print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr)) p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn) targetPos, targetOrn = p.getBasePositionAndOrientation(target) # p.resetDebugVisualizerCamera(distance, yaw, 20, targetPos) loop += 1 if loop > 10: loop = 0 data_string = "" for i in range(1, EndEffectorIndex): jt = p.getJointState(body, i) data_string += "motor%i:%i\r\n" % ( i, scale(motorDir[i - 1] * jt[0])) transfer_socket.sendto(bytes(data_string, "utf-8"), (transfer_ip, transfer_port)) ls = p.getLinkState(body, EndEffectorIndex) targetPos, targetOrn = p.getBasePositionAndOrientation(target) if (hasPrevPose): p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1, trailDuration) p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration) prevPose = targetPos prevPose1 = ls[4] hasPrevPose = 1 # get mouse events mouseEvents = p.getMouseEvents() for e in mouseEvents: if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)): mouseX = e[1] mouseY = e[2] rayFrom, rayTo = getRayFromTo(mouseX, mouseY) rayInfo = p.rayTest(rayFrom, rayTo) for l in range(len(rayInfo)): hit = rayInfo[l] objectUid = hit[0] jointUid = hit[1] if (objectUid >= 1): # this is for debug click on an object to get id # oject will change color this has no effect # changing color real time seems to slow print("obj %i joint %i" % (objectUid, jointUid)) # n = p.getJointInfo(objectUid,jointUid) n = p.getJointState(objectUid, jointUid) print(n) p.changeVisualShape(objectUid, jointUid, rgbaColor=colors[currentColor]) currentColor += 1 if (currentColor >= len(colors)): currentColor = 0 except KeyboardInterrupt: print("KeyboardInterrupt has been caught.") finally: endProgramm()
def get_body_from_pb_id(i): return p.getBodyUniqueId(i, physicsClientId=CLIENT)
def get_bodies(): return [ p.getBodyUniqueId(i, physicsClientId=CLIENT) for i in range(p.getNumBodies(physicsClientId=CLIENT)) ]