def apply_external_disturbances(self): #Apply external disturbance force if np.random.rand() < self.config["disturbance_frequency"]: self.current_disturbance = { "vector": np.array([ 2 * np.random.rand() - 1.0, 2 * np.random.rand() - 1.0, 0.4 * np.random.rand() - 0.2 ]), "remaining_life": np.random.randint(10, 40), "effect": np.random.choice(["translation", "rotation"]) } #self.current_disturbance["visual_shape"] = p.createVisualShape() if self.current_disturbance is None: return if self.current_disturbance["effect"] == "translation": p.applyExternalForce(self.robot, linkIndex=-1, forceObj=self.current_disturbance["vector"] * self.config["disturbance_intensity"], posObj=[0, 0, 0], flags=p.LINK_FRAME) else: p.applyExternalTorque( self.robot, linkIndex=-1, torqueObj=self.current_disturbance["vector"] * self.config["disturbance_intensity"] * 0.15, flags=p.LINK_FRAME) if self.current_disturbance is not None: self.current_disturbance["remaining_life"] -= 1 if self.current_disturbance["remaining_life"] <= 0: #p.removeBody(self.current_disturbance["visual_shape"]) self.current_disturbance = None
def test_pd_controller(): c = CubePD() cube = env.unwrapped.platform.cube obs = env.reset() p.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=0, cameraPitch=-40, cameraTargetPosition=[0, 0, 0]) robot_pos = obs['robot_position'] obs['object_position'] = np.array([0, 0, 0.0325]) goal_pos = obs['goal_object_position'] goal_ori = obs['goal_object_orientation'] done = False while not done: force, torque = c(goal_pos, goal_ori, obs['object_position'], obs['object_orientation']) p.applyExternalForce(objectUniqueId=cube.block, linkIndex=-1, forceObj=force, posObj=np.zeros(3), flags=p.LINK_FRAME) p.applyExternalTorque(objectUniqueId=cube.block, linkIndex=-1, torqueObj=torque, flags=p.LINK_FRAME) obs, _, done, _ = env.step(robot_pos) time.sleep(0.01)
def apply_torque_to_body(self, uid, force, position): pybullet.applyExternalTorque(objectUniqueId=uid, linkIndex=-1, forceObj=list(force), posObj=list(position), flags=pybullet.LINK_FRAME, physicsClientId=self.uid)
def set_speeds(self, speeds=[0,0,0,0]): self.speeds = np.array(speeds) forces = np.array(self.speeds**2) * self.attributes["Kf"] torques = np.array(self.speeds**2) * self.attributes["Km"] z_torque = (-torques[0] + torques[1] - torques[2] + torques[3]) for i in range(4): p.applyExternalForce(self.uid, linkIndex=i, forceObj=[0,0,forces[i]], posObj=[0,0,0], flags=p.LINK_FRAME, physicsClientId=self.sim.id) p.applyExternalTorque(self.uid, linkIndex=4, torqueObj=[0,0,z_torque], flags=p.LINK_FRAME, physicsClientId=self.sim.id)
def moveDrone(self, time_interval, j): self.detect_obstacle() self.old_pos, self.old_theta = self.pos, self.theta # Update self state self.pos, self.quat = pbl.getBasePositionAndOrientation(self.droneID) self.theta = pbl.getEulerFromQuaternion(self.quat) self.state = np.array([i for i in self.pos] + [i for i in self.theta]) self.all_state = np.vstack((self.all_state, self.state)) self.planner.all_distances = np.vstack( (self.planner.all_distances, np.array( [obstacle.getDistance(self) for obstacle in self.obstacles]))) self.thetadot = np.subtract(self.theta, self.old_theta) / time_interval self.xdot = np.subtract(self.pos, self.old_pos) / time_interval self.all_xdot.append(self.xdot) self.pathLength += np.linalg.norm(np.subtract(self.pos, self.old_pos)) start = time() if not self.targetReached: if not self.approachingTarget: self.setDesState() if abs(np.mean(self.thetadot) <= 0.05) and abs( np.mean(self.xdot) <= 0.05) and np.linalg.norm( self.controller.error[0:3] ) <= 0.2 and self.approachingTarget: self.controller.des_state = [ i for i in self.all_state[-1, 0:2] ] + [0., 0., 0., 0.] self.targetReached = True self.times.append( time() - start ) # At the end we average self.times to get the planner calculation time self.getInputs() self.all_input.append(np.array(self.input)) # The following is the actual application of the forces to the drone in simulation to make the drone move pbl.applyExternalForce(self.droneID, -1, self.thrust() - self.kd * self.xdot, [0, 0, 0], pbl.LINK_FRAME) pbl.applyExternalTorque(self.droneID, -1, self.torque(), pbl.LINK_FRAME) self.time += time_interval # ITSE formula self.e += time_interval * self.time * (np.linalg.norm( np.subtract(self.setpoint, self.pos))**2)
def step(self): self.update_state() ctrl_pitch = np.clip( self.pid_pitch_rate.step(self.setpoint_pitch_rate, self.ang_speed[1], self.cfg['dt']), *self.cfg['limits']['pid_pitch_rate_out']) ctrl_roll = np.clip( self.pid_roll_rate.step(self.setpoint_roll_rate, self.ang_speed[0], self.cfg['dt']), *self.cfg['limits']['pid_roll_rate_out']) ctrl_yaw = np.clip( self.pid_yaw_rate.step(self.setpoint_yaw_rate, self.ang_speed[2], self.cfg['dt']), *self.cfg['limits']['pid_yaw_rate_out']) translation_forces = np.clip( np.array([ self.thrust + ctrl_roll - ctrl_pitch, self.thrust - ctrl_roll - ctrl_pitch, self.thrust + ctrl_roll + ctrl_pitch, self.thrust - ctrl_roll + ctrl_pitch, ]) + np.random.uniform(-self.cfg['force_motor_noise'], self.cfg['force_motor_noise'], 4), *self.cfg['limits']['motor_force']) yaw_torque = np.clip(ctrl_yaw, *self.cfg['limits']['yaw_torque']) motor_points = np.array([[1, 1, 0], [1, -1, 0], [-1, 1, 0], [-1, -1, 0]]) * 0.2 for force_point, force in zip(motor_points, translation_forces): p.applyExternalForce(self.body_id, -1, [0, 0, force], force_point, p.LINK_FRAME, physicsClientId=self.pybullet_client) p.applyExternalTorque(self.body_id, 0, [0, 0, yaw_torque], p.LINK_FRAME, physicsClientId=self.pybullet_client) # apply noise and wind p.applyExternalForce(self.body_id, -1, (np.random.rand(3) - 0.5) * self.cfg['force_noise_fac'], self.position, p.WORLD_FRAME, physicsClientId=self.pybullet_client) p.applyExternalTorque(self.body_id, 0, (np.random.rand(3) - 0.5) * self.cfg['torque_noise_fac'], p.LINK_FRAME, physicsClientId=self.pybullet_client) p.applyExternalForce(self.body_id, -1, self.simulated_wind_vector, self.position, p.WORLD_FRAME, physicsClientId=self.pybullet_client)
def update(self, action): """Call the pybullet function to apply the desired force to this model. """ self.rotor_speed = self.rotor_speed + ( action - self.rotor_speed) * self.spool_up_rate p.applyExternalForce(self.uid, self.frame_id, [0, 0, self.max_thrust * self.rotor_speed], [0, 0, 0], p.LINK_FRAME) p.applyExternalTorque(self.uid, self.frame_id, [0, 0, self.max_torque * self.rotor_speed], p.LINK_FRAME)
def applyExternalTorque(objectUniqueId: int, torqueObj: Vec3, frame: Frame): assert isinstance(torqueObj, Vec3) # There is a bug in Pybullet that the Link Frame and World frame are swapped when applying a torque to the # base link of a robot (https://github.com/bulletphysics/bullet3/issues/1949) if frame == Frame.WORLD_FRAME: frame = Frame.LINK_FRAME else: frame = Frame.WORLD_FRAME p.applyExternalTorque(objectUniqueId, -1, torqueObj.as_nwu(), flags=frame.value)
def apply_torque(self, object_name, torque): """ Applies a 3D torque on a particular object. :param object_name: Name identifier of the object. :param force: 3D torque to apply as a KDL Vector. :return: nothing """ object_id = self.get_object_with_name(object_name) torque_list = kdl_vector_to_list(torque) link_id = -1 # print object_id, torque_list, link_id p.applyExternalTorque(object_id, link_id, torque_list, p.LINK_FRAME)
def update_physics(delay, quadcopterId, quadcopter_controller): while quadcopter_controller.sim: start = time.perf_counter() # the controllers are evaluated at a slower rate, only once in the # control_subsample times the controller is evaluated if quadcopter_controller.sample == 0: quadcopter_controller.sample = control_subsample pos_meas, quaternion_meas = p.getBasePositionAndOrientation( quadcopterId) force_act1, force_act2, force_act3, force_act4, moment_yaw = quadcopter_controller.update_control( pos_meas, quaternion_meas) print(force_act1, type(force_act1), "HEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE") else: quadcopter_controller.sample -= 1 # apply forces/moments from controls etc: # (do this each time, because forces and moments are reset to zero after a stepSimulation()) p.applyExternalForce(quadcopterId, -1, force_act1, [arm_length, 0., 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act2, [0., arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act3, [-arm_length, 0., 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act4, [0., -arm_length, 0.], p.LINK_FRAME) # for the yaw-control: p.applyExternalTorque(quadcopterId, -1, [0., 0., moment_yaw], p.LINK_FRAME) # do simulation with pybullet: p.stepSimulation() # delay than repeat calc_time = time.perf_counter() - start if (calc_time > 1.2 * delay): #print("Time to update physics is {} and is more than 20% of the desired update time ({}).".format(calc_time,delay)) pass else: # print("calc_time = ",calc_time) while (time.perf_counter() - start < delay): time.sleep(delay / 10)
def update_physics(delay, quadcopterId, quadcopter_controller): while quadcopter_controller.sim: # start = time.clock()#time.perf_counter(); # the controllers are evaluated at a slower rate, only once in the # control_subsample times the controller is evaluated # if quadcopter_controller.sample == 0: # quadcopter_controller.sample = control_subsample # pos_meas,quaternion_meas = p.getBasePositionAndOrientation(quadcopterId) # force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas) # print force_act1,force_act2,force_act3,force_act4,moment_yaw # else: # quadcopter_controller.sample -= 1 pos_meas, quaternion_meas = p.getBasePositionAndOrientation( quadcopterId) force_act1, force_act2, force_act3, force_act4, moment_yaw = quadcopter_controller.update_control( pos_meas, quaternion_meas) # print "LOOK: ", force_act1,force_act2,force_act3,force_act4,moment_yaw # apply forces/moments from controls etc: # (do this each time, because forces and moments are reset to zero after a stepSimulation()) p.applyExternalForce(quadcopterId, -1, force_act1, [arm_length, 0., 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act2, [0., arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act3, [-arm_length, 0., 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act4, [0., -arm_length, 0.], p.LINK_FRAME) # joint_state = p.getJointState(quadcopterId,0) # print joint_state # for the yaw-control: p.applyExternalTorque(quadcopterId, -1, [0., 0., moment_yaw], p.LINK_FRAME) # do simulation with pybullet: p.stepSimulation()
def step(self, action): # Forward prop neural network to get GRF, use that to change the gravity # Shoud really compute after every p.stepSimulation ankle_position, ankle_angular_velocity, appliedTorque, foot_x, foot_y, foot_z, foot_dx, foot_dy, foot_dz, foot_roll, foot_pitch, foot_yaw = self.getFootState( ) #depth = plate_bottom_z - bed_z gamma = np.sqrt(foot_dy**2 + foot_dz**2) beta = foot_roll if (self.visualizeTrajectory): self.plotPosition() customGRF = False if (foot_z < self.granularDepth and foot_z > 0.0001): #customGRF = True pass #grf_y, grf_z, torque = self.computeGRF(gamma, beta, foot_z, foot_dx, foot_dy, foot_dz, ankle_angular_velocity) # Step forward some finite number of seconds or milliseconds self.controller(action[0]) for i in range(3): foot_index = 3 # Must call this each time we stepSimulation if (customGRF): p.applyExternalForce(self.hopper, foot_index, [0, grf_y, grf_z], [0.0, 0.0, 0.0], p.LINK_FRAME) p.applyExternalTorque(self.hopper, foot_index, [torque, 0, 0], p.LINK_FRAME) p.stepSimulation() self.planarConstraint() isOver = self.checkForEnd() return self.computeObservation(), self.computeReward( isOver), isOver, None
def test_torque_gains(gains, force_gains): c = CubePD(force_gains=force_gains, torque_gains=gains) obs = env.reset() cube = env.unwrapped.platform.cube p.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=0, cameraPitch=-40, cameraTargetPosition=[0, 0, 0]) # HARD EXAMPLE init_pos = np.array([0., 0., 0.0325]) init_ori = np.array([0.0, 0.0, -1.5634019394850234]) goal_pos = np.array([-0.04638887, -0.00215872, 0.06769445]) goal_ori = np.array( [-2.2546160857726445, 0.5656166523145225, 1.9566104949225405]) goal_ori = np.array(p.getQuaternionFromEuler(goal_ori)) robot_pos = obs['robot_position'] obs['object_position'] = init_pos cube.set_state(init_pos, p.getQuaternionFromEuler(init_ori)) total_error = 0 for _ in range(1000): force, torque = c(goal_pos, goal_ori, obs['object_position'], obs['object_orientation']) p.applyExternalForce(objectUniqueId=cube.block, linkIndex=-1, forceObj=force, posObj=np.zeros(3), flags=p.LINK_FRAME) p.applyExternalTorque(objectUniqueId=cube.block, linkIndex=-1, torqueObj=torque, flags=p.LINK_FRAME) obs, _, _, _ = env.step(robot_pos) total_error += c._rotation_error( goal_ori, obs['object_orientation']).magnitude() return total_error
def step(self, contestview=False): """ does one step in the simulation """ # current time self.t = self.time_step * self.dt # get position of all drones all_pos = [] for drone in self.drones: pos, ori = pybullet.getBasePositionAndOrientation(drone['id']) all_pos.append(pos) all_pos = np.array(all_pos) all_done = True for index, drone in enumerate(self.drones): # ignore the drone if it is not still running if not drone['running']: continue # check if the drone has just now finished, and if so ignore it if self.check_ring(drone): drone['running'] = False continue # the drone is not finished, so the simulation should continue all_done = False # get state pos, rpy, linvel, angvel = self.get_state(drone) # get measurements pos_meas, rpy_meas, pos_ring, is_last_ring = self.get_sensor_measurements( drone) # get actuator commands try: tau_x_des, tau_y_des, tau_z_des, f_z_des = drone[ 'controller'].run(pos_meas, rpy_meas, pos_ring, is_last_ring, np.delete(all_pos, index, axis=0)) xhat = drone['controller'].xhat.flatten() tau_x, tau_y, tau_z, f_z = self.set_actuator_commands( tau_x_des, tau_y_des, tau_z_des, f_z_des, drone) except Exception as err: print( f'\n==========\nerror on run of drone {drone["name"]} (turning it off):\n==========\n{traceback.format_exc()}==========\n' ) drone['running'] = False continue # apply rotor forces pybullet.applyExternalForce(drone['id'], 0, np.array([0., 0., drone['u'][3]]), np.array([0., 0., 0.]), pybullet.LINK_FRAME) # apply rotor torques pybullet.applyExternalTorque( drone['id'], 0, np.array([drone['u'][0], drone['u'][1], drone['u'][2]]), pybullet.LINK_FRAME) # log data data = drone['data'] data['t'].append(self.t) data['pos'].append(pos.tolist()) data['rpy'].append(rpy.tolist()) data['linvel'].append(linvel.tolist()) data['angvel'].append(angvel.tolist()) data['pos_meas'].append(pos_meas.tolist()) data['rpy_meas'].append(rpy_meas.tolist()) data['pos_ring'].append(pos_ring.tolist()) data['is_last_ring'].append(is_last_ring) data['xhat'].append(xhat.tolist()) data['tau_x'].append(tau_x) data['tau_y'].append(tau_y) data['tau_z'].append(tau_z) data['f_z'].append(f_z) if hasattr(drone['controller'], 'user_data'): for key, val in drone['controller'].user_data.items(): if key in data['user_data'].keys(): data['user_data'][key].append(val) else: data['user_data'][key] = [val] # try to stay real-time if self.display: t = self.start_time + (self.dt * (self.time_step + 1)) time_to_wait = t - time.time() while time_to_wait > 0: time.sleep(0.9 * time_to_wait) time_to_wait = t - time.time() # take a simulation step pybullet.stepSimulation() # increment time step self.time_step += 1 # update camera if contestview: self.camera_update_contest() else: self.camera_update() return all_done
ftz = get_ftz_from_F_c(F_c) # dV_s = np.array([[0, 0, 0, 1, 0, 0]]).T # dw_c = s["R_cs"] @ dV_s[0:3] # dv_c = s["R_cs"] @ dV_s[3:] # dV_c = np.concatenate((dw_c, dv_c)) # ftz = get_ftz_from_V_c_dV_c(s["V_c"], dV_c.squeeze()) # Normalize if needed # ftz = normalize_ftz(ftz) if damping: # Apply damping from water M_c_damping = -1 * s["w_c"] p.applyExternalTorque(robot_id, -1, M_c_damping.tolist(), flags=p.WORLD_FRAME) # TODO bug in pybullet. # TODO WORLD_FRAME and link frame are inverted https://github.com/bulletphysics/bullet3/issues/1949 F_c_damping = -10 * s["v_c"] p.applyExternalForce(robot_id, -1, F_c_damping.tolist(), [0, 0, 0], flags=p.LINK_FRAME) # TODO add buoyancy force and torque. for link_idx, ftz_i in enumerate(np.nditer(ftz)): # Apply control forces p.applyExternalForce(robot_id, link_idx, [0, 0, ftz_i], [0, 0, 0], flags=p.LINK_FRAME) # Visualize p_zero_t = np.array([0, 0, 0, 1]) p_fz_t = np.array([0, 0, ftz_i, 1])
#### Apply y-axis torque to the base ############################################################### # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,5e-6,0.], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) #### Apply z-axis torque to the base ############################################################### # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) # p.applyExternalTorque(DRONE_IDS[0], linkIndex=-1, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) #### To propeller 0 ################################################################################ # p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) # p.applyExternalTorque(DRONE_IDS[0], linkIndex=0, torqueObj=[0.,0.,5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) #### To the center of mass ######################################################################### # p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0.,0.,5e-6], flags=p.WORLD_FRAME, physicsClientId=PYB_CLIENT) p.applyExternalTorque(DRONE_IDS[0], linkIndex=4, torqueObj=[0., 0., 5e-6], flags=p.LINK_FRAME, physicsClientId=PYB_CLIENT) #### Step, sync the simulation, printout the state ################################################# p.stepSimulation(physicsClientId=PYB_CLIENT) sync(i, START, env.TIMESTEP) if i % env.SIM_FREQ == 0: env.render() #### Reset ######################################################################################### if i > 1 and i % ((ARGS.duration_sec / (ARGS.num_resets + 1)) * env.SIM_FREQ) == 0: env.reset() p.setGravity(0, 0, 0, physicsClientId=PYB_CLIENT) #### Close the environment #########################################################################
thrust_output = tm.thruster_output(desired_force, desired_torque) # if debug: # print(f"Thruster Outputs: {np.around(thrust_output, 4)}") robot_pos, robot_ori = p.getBasePositionAndOrientation(sim.robot) t_vel, r_vel = p.getBaseVelocity(sim.robot) t_vel, r_vel = np.array(t_vel), np.array(r_vel) t_drag = -100.0 * np.sqrt(t_vel.dot(t_vel)) * t_vel r_drag = -3.0 * np.array(r_vel) if debug: p.addUserDebugLine(robot_pos, robot_pos + t_vel * 2, [1, 0, 0], lifeTime=0, lineWidth=3, replaceItemUniqueId=vel_line) p.applyExternalTorque(sim.robot, -1, r_drag, flags=p.LINK_FRAME) p.applyExternalForce(sim.robot, -1, t_drag, robot_pos, flags=p.WORLD_FRAME) if debug: p.addUserDebugLine(sim.com_position(), sim.com_position() + np.array([0, 0, 0.2]), [1, 0, 0], lifeTime=0, lineWidth=2, replaceItemUniqueId=com_line) p.addUserDebugLine(cob_position(), cob_position() + np.array([0, 0, 0.2]), [0, 1, 0], lifeTime=0, lineWidth=2, replaceItemUniqueId=bou_line) # print(f"Distance Between COM and COB: {sim.com_position() - cob_position()}") for thrust_num, joint in enumerate(thrusters): state = p.getLinkState(sim.robot, joint, computeLinkVelocity=0) joint_pos = np.array(state[0]) thrust_axis = np.matmul(np.array(p.getMatrixFromQuaternion(state[1])).reshape(3, 3), np.array([0, 0, 1])) thrust_vec = thrust_axis / mag(thrust_axis) * thrust_output[thrust_num] if mag(thrust_vec) > (3.71 * 1.01):
p.applyExternalForce(body, i, force[0], [0, 0, 0], flags=p.LINK_FRAME) else: for i in range(-1, p.getNumJoints(body)): pose = get_world_inertial_pose(body, i) p.applyExternalForce(body, i, [0, 0, 10], pose[0], flags=p.WORLD_FRAME) if test_case == 1: if apply_torque_local: p.applyExternalTorque(body, -1, [0, 0, 100], flags=p.LINK_FRAME) else: p.applyExternalTorque(body, -1, [0, 0, 100], flags=p.WORLD_FRAME) else: if apply_torque_local: p.applyExternalTorque(body, -1, [0, 0, 100], flags=p.LINK_FRAME) p.applyExternalTorque(body, 0, [0, 0, 100], flags=p.LINK_FRAME) else: p.applyExternalTorque(body, -1, [0, 0, 100], flags=p.WORLD_FRAME)
def test_simulator_friction_isometry(): import os from tampc import cfg import pybullet_data init_block_pos = [0.0, 0.0] init_block_yaw = -0. physics_client = p.connect(p.GUI) p.setTimeStep(1. / 240.) p.setRealTimeSimulation(False) p.setAdditionalSearchPath(pybullet_data.getDataPath()) blockId = p.loadURDF(os.path.join(cfg.ROOT_DIR, "block_big.urdf"), tuple(init_block_pos) + (-0.02, ), p.getQuaternionFromEuler([0, 0, init_block_yaw])) planeId = p.loadURDF("plane.urdf", [0, 0, -0.05], useFixedBase=True) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.resetDebugVisualizerCamera(cameraDistance=0.5, cameraYaw=0, cameraPitch=-85, cameraTargetPosition=[0, 0, 1]) p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "test_invariant.mp4") STATIC_VELOCITY_THRESHOLD = 1e-6 def _observe_block(blockId): blockPose = p.getBasePositionAndOrientation(blockId) xb = blockPose[0][0] yb = blockPose[0][1] roll, pitch, yaw = p.getEulerFromQuaternion(blockPose[1]) return np.array((xb, yb, yaw)) def _static_environment(): v, va = p.getBaseVelocity(blockId) if (np.linalg.norm(v) > STATIC_VELOCITY_THRESHOLD) or ( np.linalg.norm(va) > STATIC_VELOCITY_THRESHOLD): return False return True p.setGravity(0, 0, -10) # p.changeDynamics(blockId, -1, lateralFriction=0.1) p.changeDynamics(planeId, -1, lateralFriction=0.5, spinningFriction=0.3, rollingFriction=0.1) f_mag = 1000 f_dir = np.pi / 4 ft = math.sin(f_dir) * f_mag fn = math.cos(f_dir) * f_mag MAX_ALONG = 0.075 + 0.2 for _ in range(100): p.stepSimulation() N = 300 yaws = np.zeros(N) z_os = np.zeros((N, 3)) for simTime in range(N): # observe difference from pushing px = _observe_block(blockId) yaws[simTime] = px[2] # p.applyExternalForce(blockId, -1, [fn, ft, 0], [-MAX_ALONG, MAX_ALONG, 0.025], p.LINK_FRAME) p.applyExternalTorque(blockId, -1, [0, 0, 150], p.LINK_FRAME) p.stepSimulation() while not _static_environment(): for _ in range(100): p.stepSimulation() cx = _observe_block(blockId) # difference in world frame dx = get_dx(px, cx) # difference in block frame dz = dx_to_dz(px, dx) z_os[simTime] = dz logger.info("dx %s dz %s", dx, dz) time.sleep(0.1) plot_and_analyze_body_frame_invariance(yaws, z_os)
import os, inspect currentdir = os.path.dirname( os.path.abspath(inspect.getfile(inspect.currentframe()))) parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0, parentdir) import time import pybullet as p import pybullet_data p.connect(p.GUI) timeStep = 0.01 p.setRealTimeSimulation(0) p.setTimeStep(timeStep) p.resetSimulation() cube0 = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"), [0, 0, 0]) cube1 = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"), [2, 0, 0], [0, 0.7071, 0, 0.7071]) cube2 = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"), [0, 2, 0]) cube3 = p.loadURDF(os.path.join(pybullet_data.getDataPath(), "r2d2.urdf"), [2, 2, 0], [0, 0.7071, 0, 0.7071]) while 1: p.stepSimulation() time.sleep(timeStep) p.applyExternalTorque(cube0, -1, [1, 0, 0], p.LINK_FRAME) p.applyExternalTorque(cube1, -1, [1, 0, 0], p.LINK_FRAME) p.applyExternalTorque(cube2, -1, [1, 0, 0], p.WORLD_FRAME) p.applyExternalTorque(cube3, -1, [1, 0, 0], p.WORLD_FRAME)
def apply_external_torque_to_base(self, torque, robot_index=1): p.applyExternalTorque(robot_index, self.torso_ids[robot_index], torque, flags=p.WORLD_FRAME)
p.stepSimulation() time.sleep(1. / 240.) if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)): for joint in range(2, 6): p.setJointMotorControl2(car, joint, p.VELOCITY_CONTROL, targetVelocity=targetVelS, force=maxForce) p.stepSimulation() time.sleep(1. / 240.) if (k == ord('r') and (v & p.KEY_IS_DOWN)): p.applyExternalTorque(car, -1, [0, 0, 180], flags=p.LINK_FRAME) p.stepSimulation() time.sleep(1. / 240.) if (k == ord('r') and (v & p.KEY_WAS_RELEASED)): p.applyExternalTorque(car, -1, [0, 0, 0], flags=p.LINK_FRAME) p.stepSimulation() time.sleep(1. / 240.) if (k == ord('a') and (v & p.KEY_IS_DOWN)): targetVel = targetVel + 1 targetVel1 = targetVel1 + 0.5 targetVel2 = targetVel2 - 0.5 targetVelRev = targetVelRev - 1 while (1): br = 0
def update_physics(self, delay, quadcopterId, quadcopter_controller, cmd_sub, imu_pub, range_pub): vol_meas = p.getBaseVelocity(quadcopterId) one_over_sample_rate = 1 / delay previous_publish = time.perf_counter() try: while not rospy.is_shutdown(): start = time.perf_counter() # the controllers are evaluated at a slower rate, only once in the # control_subsample times the controller is evaluated pos_meas, quaternion_meas = p.getBasePositionAndOrientation( quadcopterId) previous_vol_meas = np.array(vol_meas) vol_meas = np.array(p.getBaseVelocity(quadcopterId)) #print(vol_meas, type(vol_meas)) acc = (vol_meas - previous_vol_meas) * one_over_sample_rate #print("VOL MEAS", previous_vol_meas, acc) if quadcopter_controller.sample == 0: quadcopter_controller.sample = control_subsample #force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas) actual_values = cmd_sub.get_control() vectorizer = np.zeros([5, 3]) vectorizer[:, 2] = actual_values #print("input forces ", actual_values) force_act1, force_act2, force_act3, force_act4, moment_yaw = vectorizer #print("Well well well\t\t", vectorizer, force_act1, moment_yaw) else: quadcopter_controller.sample -= 1 # apply forces/moments from controls etc: # (do this each time, because forces and moments are reset to zero after a stepSimulation()) #assuming right is -y p.applyExternalForce(quadcopterId, -1, force_act1, [arm_length, -arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act2, [-arm_length, -arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act3, [-arm_length, arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act4, [arm_length, arm_length, 0.], p.LINK_FRAME) # for the yaw-control: p.applyExternalTorque(quadcopterId, -1, moment_yaw, p.LINK_FRAME) # do simulation with pybullet: p.stepSimulation() header = Header() header.frame_id = 'Body' header.stamp = rospy.Time.now() Tsample_physics imu_message = Imu() imu_message.header = header imu_message.orientation.x = quaternion_meas[0] imu_message.orientation.y = quaternion_meas[1] imu_message.orientation.z = quaternion_meas[2] imu_message.orientation.w = quaternion_meas[3] imu_message.linear_acceleration.x = acc[0, 0] imu_message.linear_acceleration.y = acc[0, 1] imu_message.linear_acceleration.z = acc[0, 2] msg = Range() msg.header = header msg.max_range = 200 msg.min_range = 0 msg.range = pos_meas[2] if start - previous_publish > 1 / 100: imu_pub.publish(imu_message) range_pub.publish(msg) previous_publish = start #print("BUBLISHED RANON /simulation/infrared_sensor_node", msg.range) # delay than repeat calc_time = time.perf_counter() - start if (calc_time > delay): #print("Time to update physics is {} and is more than 20% of the desired update time ({}).".format(calc_time,delay)) pass else: # print("calc_time = ",calc_time) while (time.perf_counter() - start < delay): time.sleep(delay / 10) except KeyboardInterrupt: self.ctrl_c_reset()
def _step(self, action): p.stepSimulation() # This is to controll manually the quad. if self._renders: time.sleep(self.timeStep) keys = p.getKeyboardEvents() for k, v in keys.items(): if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_TRIGGERED)): self.speedX = 0.01 if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_WAS_RELEASED)): self.speedX = 0 if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_TRIGGERED)): self.speedX = -0.01 if (k == p.B3G_LEFT_ARROW and (v & p.KEY_WAS_RELEASED)): self.speedX = 0 if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_TRIGGERED)): self.forward = 0.05 if (k == p.B3G_UP_ARROW and (v & p.KEY_WAS_RELEASED)): self.forward = 0.0 if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_TRIGGERED)): self.forward = -0.05 if (k == p.B3G_DOWN_ARROW and (v & p.KEY_WAS_RELEASED)): self.forward = 0.0 self.dx = self.dx + self.speedX #self.offset_command = self.offset_command + self.forward print(self.dx) #print(keys) if self._renders: p.resetBasePositionAndOrientation(self.desired_pos_sphere, [self.dx, 0, 0], [0, 0, 0, 1]) #if self._renders: #print(math.fabs(x)/2.4) #print(math.fabs(self.acceleration)*1) world_pos, world_ori = p.getBasePositionAndOrientation(self.quad) world_pos_offset = tuple([world_pos[0] - self.dx]) + world_pos[1:3] world_vel, world_rot_vel = p.getBaseVelocity(self.quad) # world_rot_vel = (1, 0, 0) # To obtain local rot_vel # Expressing world rotation in quadcopter frame coordinate, required for PD controller. local_rot_vel = qt.qv_mult(qt.q_conjugate(world_ori), world_rot_vel) desired_rot_vel = np.array(action[1:4]) * 5 rot_error = np.asarray(local_rot_vel) - desired_rot_vel self.lpf_rot_error = rot_error * 0.05 + self.lpf_rot_error * 0.95 thrust = (action[0] + 1) * self.zero_thrust torque = -rot_error * 0.1 ang_power = np.sum(np.abs(np.multiply(torque, local_rot_vel))) thrust_change = math.fabs((self.last_thrust - thrust)) p.applyExternalTorque(self.quad, -1, -rot_error * 0.1, p.WORLD_FRAME) p.applyExternalForce(self.quad, -1, [0, 0, thrust], [0, 0, 0], p.LINK_FRAME) contacts = p.getContactPoints() self.state = world_pos_offset + world_ori + world_vel + world_rot_vel # print(self.state) touched_ground = 0 if contacts != (): touched_ground = -100 distance_from_center = np.linalg.norm( np.asarray(world_pos) - np.asarray([0, 0, 0.5])) reward = 1 - distance_from_center * 0.5 - ang_power * 0.14 - thrust_change * 0.05 + touched_ground # print(rot_error) done = 0 if contacts != (): done = 1 if distance_from_center > 1: done = 1 return np.array(self.state), reward, done, {}
def check_stability(verts, faces, gui=False): # First, check if the file is even rooted. # If it's not rooted, it can't be stable if not check_rooted(verts, faces): 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(verts, faces, 'tmp') objIds = [] startPositions = {} volumes = {} heights = {} bases = {} 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 volumes[objId] = data['volume'] heights[objId] = data['height'] bases[objId] = data['base'] 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) import math # See if objects are stable under some perturbation for objId in objIds: s = volumes[objId] b = bases[objId] v = s * b # 800, 4, 4, 4 p.applyExternalForce(objId, -1, (0, 0, 1000*v), startPositions[objId], p.WORLD_FRAME) p.applyExternalTorque(objId, -1, (0, 4*v, 0), p.WORLD_FRAME) p.applyExternalTorque(objId, -1, (4*v, 0, 0), p.WORLD_FRAME) p.applyExternalTorque(objId, -1, (0, 0, 80*v), p.WORLD_FRAME) # Run simulation if gui: for i in range(600): p.stepSimulation() time.sleep(1./600.) 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(zstart - zend) if zdiff > abs(0.025 * heights[objId]): p.disconnect() return False p.disconnect() return True
def step(self, ctrl_raw): # Add new action to queue self.act_queue.append(ctrl_raw) self.act_queue.pop(0) # Simulate the delayed action if self.randomized_params["output_transport_delay"] > 0: ctrl_raw_unqueued = self.act_queue[-self.config["act_input"]:] ctrl_delayed = self.act_queue[ -1 - self.randomized_params["output_transport_delay"]] else: ctrl_raw_unqueued = self.act_queue ctrl_delayed = self.act_queue[-1] # Scale the action appropriately (neural network gives [-1,1], pid controller [0,1]) if self.config["controller_source"] == "nn": ctrl_processed = np.clip(ctrl_delayed, -1, 1) * 0.5 + 0.5 else: ctrl_processed = ctrl_delayed # Take into account motor delay self.update_motor_vel(ctrl_processed) # Apply motor forces for i in range(4): motor_force_w_noise = np.clip( self.current_motor_velocity_vec[i] * self.randomized_params["motor_power_variance_vector"][i], 0, 1) motor_force_scaled = motor_force_w_noise * self.randomized_params[ "motor_force_multiplier"] p.applyExternalForce(self.robot, linkIndex=i * 2 + 1, forceObj=[0, 0, motor_force_scaled], posObj=[0, 0, 0], flags=p.LINK_FRAME, physicsClientId=self.client_ID) p.applyExternalTorque( self.robot, linkIndex=i * 2 + 1, torqueObj=[ 0, 0, self.current_motor_velocity_vec[i] * self.reactive_torque_dir_vec[i] * self.config["propeller_parasitic_torque_coeff"] ], flags=p.LINK_FRAME, physicsClientId=self.client_ID) self.apply_external_disturbances() p.stepSimulation(physicsClientId=self.client_ID) if self.config["animate"]: time.sleep(self.config["sim_timestep"]) self.step_ctr += 1 torso_pos, torso_quat, torso_euler, torso_vel, torso_angular_vel = self.get_obs( ) roll, pitch, yaw = torso_euler pos_delta = np.array(torso_pos) - np.array(self.config["target_pos"]) crashed = (abs(pos_delta) > 6.0).any() or ( (torso_pos[2] < 0.3) and (abs(roll) > 2.5 or abs(pitch) > 2.5)) if self.prev_act is not None: action_penalty = np.mean( np.square(np.array(ctrl_processed) - np.array(self.prev_act)) ) * self.config["pen_act_coeff"] else: action_penalty = 0 # Calculate true reward pen_position = np.mean(np.square(pos_delta)) * ( self.config["pen_position_coeff"] + 0 * self.step_ctr / float(self.config["max_steps"])) pen_rpy = np.mean(np.square( np.array(torso_euler))) * self.config["pen_rpy_coeff"] pen_vel = np.mean(np.square(torso_vel)) * self.config["pen_vel_coeff"] pen_rotvel = np.mean( np.square(torso_angular_vel)) * self.config["pen_ang_vel_coeff"] r_true = -action_penalty - pen_position - pen_rpy - pen_rotvel - pen_vel #print(action_penalty, pen_position, pen_rpy, pen_rotvel, pen_vel) # Calculate proxy reward (for learning purposes) pen_position_proxy = np.mean( np.square(pos_delta)) * self.config["pen_position_coeff"] pen_yaw_proxy = np.mean(np.square(yaw)) * self.config["pen_yaw_coeff"] pen_vel_proxy = np.mean(np.square(torso_vel)) * np.maximum( 1. - 3 * pen_position_proxy, 0) * self.config["pen_vel_coeff"] r = -pen_position_proxy - pen_yaw_proxy - action_penalty - pen_vel_proxy r = np.clip(r_true, -2, 2) if self.step_ctr == 1: r = 0 self.rew_queue.append([r]) self.rew_queue.pop(0) if self.randomized_params["input_transport_delay"] > 0: r_unqueued = self.rew_queue[-self.config["rew_input"]:] else: r_unqueued = self.rew_queue done = (self.step_ctr > self.config["max_steps"]) compiled_obs = pos_delta, torso_quat, torso_vel, torso_angular_vel compiled_obs_flat = [ item for sublist in compiled_obs for item in sublist ] self.obs_queue.append(compiled_obs_flat) self.obs_queue.pop(0) if self.randomized_params["input_transport_delay"] > 0: obs_raw_unqueued = self.obs_queue[-self.config["obs_input"]:] else: obs_raw_unqueued = self.obs_queue aux_obs = [] for i in range(len(obs_raw_unqueued)): t_obs = [] t_obs.extend(obs_raw_unqueued[i]) if self.config["act_input"] > 0: t_obs.extend(ctrl_raw_unqueued[i]) if self.config["rew_input"] > 0: t_obs.extend(r_unqueued[i]) if self.config["step_counter"] > 0: def step_ctr_to_obs(step_ctr): return (float(step_ctr) / self.config["max_steps"]) * 2 - 1 t_obs.extend( [step_ctr_to_obs(np.maximum(self.step_ctr - i - 1, 0))]) aux_obs.extend(t_obs) if self.config["latent_input"]: aux_obs.extend(self.randomized_params_list_norm) obs = np.array(aux_obs).astype(np.float32) obs_dict = { "pos_delta": pos_delta, "torso_quat": torso_quat, "torso_vel": torso_vel, "torso_angular_vel": torso_angular_vel, "reward": r_true, "action": ctrl_processed } self.prev_act = ctrl_processed return obs, r, done, { "obs_dict": obs_dict, "randomized_params": self.randomized_params, "true_rew": r_true }
p.changeDynamics(boxId3, -1, lateralFriction=0.0, restitution=1) p.createConstraint(cube3, -1, boxId3, -1, p.JOINT_POINT2POINT, [1, 0, 0], [0, 0, 0], [0, 0, 1]) p.createConstraint(cube4, -1, boxId3, -1, p.JOINT_POINT2POINT, [1, 0, 0], [0, 0, 0], [0, 0, 1]) sphereStartPos = [0.35, 0, 1] sphereStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) boxId4 = p.loadURDF("/sphere.urdf", sphereStartPos, sphereStartOrientation) p.changeDynamics(boxId4, -1, lateralFriction=0.0, restitution=1) p.createConstraint(cube4, -1, boxId4, -1, p.JOINT_POINT2POINT, [1, 0, 0], [0, 0, 0], [0, 0, 1]) p.createConstraint(cube5, -1, boxId4, -1, p.JOINT_POINT2POINT, [1, 0, 0], [0, 0, 0], [0, 0, 1]) sphereStartPos = [0.45, 0, 1] sphereStartOrientation = p.getQuaternionFromEuler([0, 0, 0]) boxId5 = p.loadURDF("/sphere.urdf", sphereStartPos, sphereStartOrientation) p.changeDynamics(boxId5, -1, lateralFriction=0.0, restitution=1) p.createConstraint(cube5, -1, boxId5, -1, p.JOINT_POINT2POINT, [1, 0, 0], [0, 0, 0], [0, 0, 1]) p.createConstraint(cube6, -1, boxId5, -1, p.JOINT_POINT2POINT, [1, 0, 0], [0, 0, 0], [0, 0, 1]) i = 0 while True: if i < 150: p.applyExternalTorque(boxId1, -1, [0, 100, 0], flags=1) p.stepSimulation() i += 1 time.sleep(0.01) # cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) # print(cubePos,cubeOrn) p.disconnect()
def update_physics(delay, quadcopterId, quadcopter_controller, config, graph: datalogger): while quadcopter_controller.sim: #start = time.perf_counter() #start = time.clock() # the controllers are evaluated at a slower rate, only once in the # control_subsample times the controller is evaluated # if quadcopter_controller.sample == 0: # quadcopter_controller.sample = control_subsample # pos_meas,quaternion_meas = p.getBasePositionAndOrientation(quadcopterId) # force_act1,force_act2,force_act3,force_act4,moment_yaw = quadcopter_controller.update_control(pos_meas,quaternion_meas) # else: # quadcopter_controller.sample -= 1 pos_meas, quaternion_meas = p.getBasePositionAndOrientation( quadcopterId) rotmat = quaternion2rotation_matrix(quaternion_meas) front = np.array([pos_meas[0] + 0.1750, pos_meas[1], pos_meas[2]]) back = np.array([pos_meas[0] - 0.1750, pos_meas[1], pos_meas[2]]) left = np.array([pos_meas[0], pos_meas[1] + 0.1750, pos_meas[2]]) right = np.array([pos_meas[0], pos_meas[1] - 0.1750, pos_meas[2]]) front_meas = np.dot(rotmat.T, front.reshape(3, 1)).reshape(3) back_meas = np.dot(rotmat.T, back.reshape(3, 1)).reshape(3) left_meas = np.dot(rotmat.T, left.reshape(3, 1)).reshape(3) right_meas = np.dot(rotmat.T, right.reshape(3, 1)).reshape(3) force_act1, force_act2, force_act3, force_act4, moment_yaw = quadcopter_controller.update_control( pos_meas, quaternion_meas, config) radius = 0.25 ige_force1 = ige_force2 = ige_force3 = ige_force4 = 1.0 if (front_meas[2] < 5 * radius): ige_force1 = groundEffect.groundEffect(front_meas[2], radius) if (back_meas[2] < 5 * radius): ige_force3 = groundEffect.groundEffect(back_meas[2], radius) if (left_meas[2] < 5 * radius): ige_force2 = groundEffect.groundEffect(left_meas[2], radius) if (back_meas[2] < 5 * radius): ige_force4 = groundEffect.groundEffect(right_meas[2], radius) if graph.capture: graph.addpoint(pos_meas[2], force_act1[2]) force_act1 = force_act1 / ige_force1 force_act2 = force_act2 / ige_force2 force_act3 = force_act3 / ige_force3 force_act4 = force_act4 / ige_force4 #capturing forces with altitude to plot # apply forces/moments from controls etc: # (do this each time, because forces and moments are reset to zero after a stepSimulation()) p.applyExternalForce(quadcopterId, -1, force_act1, [config.arm_length, 0., 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act2, [0., config.arm_length, 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act3, [-config.arm_length, 0., 0.], p.LINK_FRAME) p.applyExternalForce(quadcopterId, -1, force_act4, [0., -config.arm_length, 0.], p.LINK_FRAME) # for the yaw-control: p.applyExternalTorque(quadcopterId, -1, [0., 0., moment_yaw], p.LINK_FRAME) # do simulation with pybullet: p.stepSimulation()
def _move_PD(self, pose_handle_base_world_des, q_offset, mech, last_traj_p, debug=False, stable_timeout=100, unstable_timeout=1000): finished = False handle_base_ps = [] for i in itertools.count(): handle_base_ps.append(mech.get_pose_handle_base_world().p) if self.use_gripper: self._control_fingers('close', debug=debug) if (not last_traj_p) and self._at_des_handle_base_pose( pose_handle_base_world_des, q_offset, mech, 0.01): return handle_base_ps, False elif last_traj_p and self._at_des_handle_base_pose( pose_handle_base_world_des, q_offset, mech, 0.000001) and self._stable(handle_base_ps): return handle_base_ps, True elif self._stable(handle_base_ps) and (i > stable_timeout): return handle_base_ps, True elif i > unstable_timeout: return handle_base_ps, True # get position error of the handle base p_handle_base_world_err, e_handle_base_world_err = self._get_pose_handle_base_world_error( pose_handle_base_world_des, q_offset, mech) # use handle vel or gripper vel to calc velocity error if not self.use_gripper: lin_v_com_world_err = p.getLinkState(mech._get_bb_id(), \ mech._get_handle_id(), computeLinkVelocity=1)[6] else: v_tip_world_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] lin_v_com_world_err, omega_com_world_err = self._get_v_com_world_error( v_tip_world_des) f = np.multiply(self.k[0], p_handle_base_world_err) + \ np.multiply(self.d[0], lin_v_com_world_err) # only apply torques if using gripper if self.use_gripper: tau = np.multiply( self.k[1], e_handle_base_world_err ) #+ np.multiply(self.d[1], omega_com_world_err) else: tau = [0, 0, 0] self.errors += [(p_handle_base_world_err, lin_v_com_world_err)] self.forces += [(f, tau)] if not self.use_gripper: bb_id = mech._get_bb_id() handle_id = mech._get_handle_id() handle_pos = mech.get_pose_handle_base_world().p handle_q = mech.get_pose_handle_base_world().q # transform the force into the LINK_FRAME to apply f = util.transformation(f, [0., 0., 0.], handle_q, inverse=True) p.applyExternalForce(bb_id, handle_id, f, [0., 0., 0.], p.LINK_FRAME) if debug: p.addUserDebugLine(handle_pos, np.add(handle_pos, 2 * (f / np.linalg.norm(f))), lifeTime=.05) else: p_com_world, q_com_world = self._get_pose_com_('world') p.applyExternalForce(self.id, -1, f, p_com_world, p.WORLD_FRAME) # there is a bug in pyBullet. the link frame and world frame are inverted # this should be executed in the WORLD_FRAME p.applyExternalTorque(self.id, -1, tau, p.LINK_FRAME) p.stepSimulation()