def _step(self, action): p.stepSimulation() # time.sleep(self.timeStep) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state force = action p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(action + self.state[3])) done = x < -self.x_threshold \ or x > self.x_threshold \ or theta < -self.theta_threshold_radians \ or theta > self.theta_threshold_radians reward = 1.0 return np.array(self.state), reward, done, {}
def getMotorTorques(self): motorTorques = [] for i in range(self.nMotors): jointState = p.getJointState(self.quadruped, self.motorIdList[i]) motorTorques.append(jointState[3]) motorTorques = np.multiply(motorTorques, self.motorDir) return motorTorques
def _step(self, action): force = self.force_mag if action==1 else -self.force_mag p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force) p.stepSimulation() self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state done = x < -self.x_threshold \ or x > self.x_threshold \ or theta < -self.theta_threshold_radians \ or theta > self.theta_threshold_radians done = bool(done) reward = 1.0 #print("state=",self.state) return np.array(self.state), reward, done, {}
def _step(self, action): p.stepSimulation() # time.sleep(self.timeStep) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] theta, theta_dot, x, x_dot = self.state dv = 0.1 deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action] p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3])) done = x < -self.x_threshold \ or x > self.x_threshold \ or theta < -self.theta_threshold_radians \ or theta > self.theta_threshold_radians reward = 1.0 return np.array(self.state), reward, done, {}
def _reset(self): # print("-----------reset simulation---------------") p.resetSimulation() self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0]) self.timeStep = 0.01 p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) p.setGravity(0,0, -10) p.setTimeStep(self.timeStep) p.setRealTimeSimulation(0) initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) p.resetJointState(self.cartpole, 1, initialAngle) p.resetJointState(self.cartpole, 0, initialCartPos) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] return np.array(self.state)
def _reset(self): # print("-----------reset simulation---------------") p.resetSimulation() self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0]) p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0) p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0) p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0) self.timeStep = 0.02 p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0) p.setGravity(0,0, -9.8) p.setTimeStep(self.timeStep) p.setRealTimeSimulation(0) randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,)) p.resetJointState(self.cartpole, 1, randstate[0], randstate[1]) p.resetJointState(self.cartpole, 0, randstate[2], randstate[3]) #print("randstate=",randstate) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] #print("self.state=", self.state) return np.array(self.state)
def GetMotorTorques(self): """Get the amount of torques the motors are exerting. Returns: Motor torques of all eight motors. """ if self._accurate_motor_model_enabled or self._pd_control_enabled: return self._observed_motor_torques else: motor_torques = [ p.getJointState(self.minitaur, motor_id)[3] for motor_id in self._motor_id_list ] motor_torques = np.multiply(motor_torques, self._motor_direction) return motor_torques
def reset(self): # print("-----------reset simulation---------------") p.resetSimulation() self.cartpole = p.loadURDF( os.path.join(pybullet_data.getDataPath(), "cartpole.urdf"), [0, 0, 0]) p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0) p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0) p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0) self.timeStep = 0.02 p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0) p.setGravity(0, 0, -9.8) p.setTimeStep(self.timeStep) p.setRealTimeSimulation(0) randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4, )) p.resetJointState(self.cartpole, 1, randstate[0], randstate[1]) p.resetJointState(self.cartpole, 0, randstate[2], randstate[3]) #print("randstate=",randstate) self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState( self.cartpole, 0)[0:2] #print("self.state=", self.state) return np.array(self.state)
def get_obs(self): x, x_dot, theta, theta_dot = p.getJointState( self.cartpole, 0)[0:2] + p.getJointState(self.cartpole, 1)[0:2] # Clip velocities x_dot = np.clip(x_dot / 3, -7, 7) theta_dot = np.clip(theta_dot / 3, -7, 7) # Change theta range to [-pi, pi] if theta > 0: if theta % (2 * np.pi) <= np.pi: theta = theta % (2 * np.pi) else: theta = -np.pi + theta % np.pi else: if theta % (-2 * np.pi) >= -np.pi: theta = theta % (-2 * np.pi) else: theta = np.pi + theta % -np.pi theta /= np.pi self.state = np.array([x, x_dot, theta, theta_dot]) return self.state
def state(self): """ :return: Current robot state, as a dictionary, containing joint positions, velocities, efforts, zero jacobian, joint space inertia tensor, end-effector position, end-effector orientation, end-effector velocity (linear and angular), end-effector force, end-effector torque :rtype: dict: {'position': np.ndarray, 'velocity': np.ndarray, 'effort': np.ndarray, 'jacobian': np.ndarray, 'inertia': np.ndarray, 'ee_point': np.ndarray, 'ee_ori': np.ndarray, 'ee_vel': np.ndarray, 'ee_omg': np.ndarray, 'tip_state'['force']: np.ndarray, 'tip_state'['torque']: np.ndarray, } """ joint_angles = self.angles() joint_velocities = self.joint_velocities() joint_efforts = self.joint_efforts() state = {} state['position'] = joint_angles state['velocity'] = joint_velocities state['effort'] = joint_efforts state['jacobian'] = self.jacobian(None) state['inertia'] = self.inertia(None) state['ee_point'], state['ee_ori'] = self.ee_pose() state['ee_vel'], state['ee_omg'] = self.ee_velocity() tip_state = {} ft_joint_state = pb.getJointState(self._id, max(self._ft_joints), physicsClientId=self._uid) ft = np.asarray(ft_joint_state[2]) tip_state['force'] = ft[:3] tip_state['torque'] = ft[3:] state['tip_state'] = tip_state return state
def get_potential(self, env): """ Compute task-specific potential: furniture joint positions :param env: environment instance :param: task potential """ task_potential = 0.0 for (body_id, joint_id) in self.body_joint_pairs: j_type = p.getJointInfo(body_id, joint_id)[2] j_pos = p.getJointState(body_id, joint_id)[0] scale = self.prismatic_joint_reward_scale \ if j_type == p.JOINT_PRISMATIC \ else self.revolute_joint_reward_scale task_potential += scale * j_pos return task_potential
def motor_joint_accelerations(self): """ returns the motor joint positions for "each DoF" according to pybullet. Note: fixed joints have 0 degrees of freedoms. """ joint_states = [] for joint_num in range(self.num_joints): joint_states.append( pybullet.getJointState(self._arm_id, joint_num, physicsClientId=self.physics_id)) # Only get joint states of free joints joint_accelerations = [state[3] for state in joint_states] return joint_accelerations
def setAngle(self, id, angle, reach_time=0): id = str(id) if not (id in self.joint_id): return present_angle = bullet.getJointState(self.humanoid_id, self.joint_id[id])[0] target_angle = math.radians(angle) max_vel = self.joint_max_vel[id] if reach_time == 0 else min( abs(target_angle - present_angle) / reach_time, self.joint_max_vel[id]) bullet.setJointMotorControl2(self.humanoid_id, jointIndex=self.joint_id[id], controlMode=bullet.POSITION_CONTROL, targetPosition=target_angle, force=7.3, maxVelocity=max_vel)
def getObservation(self): observation = [] pos, orn = p.getBasePositionAndOrientation(self.bioId) euler = p.getEulerFromQuaternion(orn) #observation.extend(list(pos)) #observation.extend(list(euler)) #roll, pitch, yaw jointStates = [] jointPoses = [] for i in range(len(self.freeJointList)): jointStates.append( p.getJointState(self.bioId, self.freeJointList[i])) jointPoses = [x[0] for x in jointStates] observation.extend(list(jointPoses)) return observation
def getAngleInterpolation(bodyID, finalAngles, k=1000): #starting angles are the angles that you begin at #final angles are the angles you want to end at #target angles is the big array that has the interpolation of all the angles between the starting and final angles num_joints = p.getNumJoints(bodyID) startingAngles = [] for i in range(num_joints): startingAngles.append(p.getJointState(bodyID, i)[0]) startingAngles = np.array(startingAngles) finalAngles = np.array(finalAngles) targetAngles = np.zeros((k, num_joints)) targetAngles[0] = startingAngles for i in range(1, k + 1): targetAngles[ i - 1] = startingAngles + (finalAngles - startingAngles) * (i / k) return targetAngles
def get_joint_torque(self, joint_uid): """Get the torque force of the joint. This is the motor torque applied during the last stepSimulation(). Args: joint_uid: A tuple of the body Unique ID and the joint index. Returns: A 3-dimensional float32 numpy array. """ body_uid, joint_ind = joint_uid _, _, _, torque = pybullet.getJointState(bodyUniqueId=body_uid, jointIndex=joint_ind, physicsClientId=self.uid) return np.array(torque, dtype=np.float32)
def _compute_observation(self): humanoid_pos, humanoid_orientation = p.getBasePositionAndOrientation( self.humanoid_id) humanoid_orientation = p.getEulerFromQuaternion(humanoid_orientation) humanoid_linear_vel, humanoid_angular_vel = p.getBaseVelocity( self.humanoid_id) joint_states = [] for j in self.joint_ids: joint_states.append(p.getJointState(self.humanoid_id, j)[0]) return np.concatenate([ humanoid_pos, humanoid_orientation, humanoid_linear_vel, humanoid_angular_vel, joint_states ])
def step(self, ctrl): p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=ctrl * 20) p.stepSimulation() self.step_ctr += 1 # x, x_dot, theta, theta_dot obs = self.get_obs() x, x_dot, theta, theta_dot = obs x_sphere = x - np.sin(p.getJointState(self.cartpole, 1)[0]) target_pen = np.clip( np.abs(x_sphere - self.target) * 3.0 * (1 - abs(theta)), -2, 2) vel_pen = (np.square(x_dot) * 0.1 + np.square(theta_dot) * 0.5) * (1 - abs(theta)) r = 1 - target_pen - vel_pen - np.square(ctrl[0]) * 0.03 #p.removeAllUserDebugItems() #p.addUserDebugText("sphere mass: {0:.3f}".format(self.mass), [0, 0, 2]) #p.addUserDebugText("sphere x: {0:.3f}".format(x_sphere), [0, 0, 2]) #p.addUserDebugText("cart pen: {0:.3f}".format(cart_pen), [0, 0, 2]) #p.addUserDebugText("x: {0:.3f}".format(x), [0, 0, 2]) #p.addUserDebugText("x_target: {0:.3f}".format(self.target), [0, 0, 2.2]) #p.addUserDebugText("cart_pen: {0:.3f}".format(cart_pen), [0, 0, 2.4]) done = self.step_ctr > self.max_steps # Change target if np.random.rand() < self.target_change_prob: self.target = np.clip( np.random.rand() * 2 * self.target_var - self.target_var, -2, 2) p.resetBasePositionAndOrientation(self.target_vis, [self.target, 0, -1], [0, 0, 0, 1]) if self.latent_input: obs = np.concatenate((obs, [self.get_latent_label()])) if self.action_input: obs = np.concatenate((obs, ctrl)) obs = np.concatenate((obs, [self.target])) return obs, r, done, {}
def get_joint_angle(self, leg_down): rid = self.robot_id if leg_down == 'left': ang_a = p.getJointState(rid, 0)[0] ang_b = p.getJointState(rid, 1)[0] ang_c = p.getJointState(rid, 2)[0] else: ang_a = p.getJointState(rid, 4)[0] ang_b = p.getJointState(rid, 5)[0] ang_c = p.getJointState(rid, 6)[0] return [ang_a, ang_b, ang_c]
def feedback(self, bodyID, state): d = self.getD(bodyID, state) v = getComVel(bodyID) #print(v) #print(d) pos_torso = p.getJointState(bodyID, 2)[0] #print("pos_torso:{}".format(pos_torso)) pose0 = np.array([-0.0, 0.0, -0.05, 0.2, 0.62 - pos_torso, -1.10, 0.2]) pose1 = np.array([-0.0, 0.0, -0.1, 0.2, -0.1 - pos_torso, -0.05, 0.2]) pose2 = np.array([-0.0, 0.62 - pos_torso, -1.10, 0.2, 0.0, -0.05, 0.2]) pose3 = np.array([-0.0, -0.1 - pos_torso, -0.05, 0.2, 0.0, -0.1, 0.2]) pose = [] pose.append(pose0) pose.append(pose1) pose.append(pose2) pose.append(pose3) for i in range(4): self.states[i].setTargetAngles(pose[i]) if (state == 1): self.states[state].setTargetAngles(pose[state]) self.states[state].targetAngles[4] += d * 2.5 #self.states[state].targetAngles[5] += d * 0.01 * np.pi #self.states[state].targetAngles[6] += d * 0.1 * np.pi #self.states[state].targetAngles[2] += d * 0.01 * np.pi if (state == 3): self.states[state].setTargetAngles(pose[state]) #self.states[state].targetAngles[0] += d * 0.03 * np.pi #self.states[state].targetAngles[4] += d * 0.01 * np.pi #self.states[state].targetAngles[5] += d * 0.01 * np.pi self.states[state].targetAngles[1] += d * 2.5 #self.states[state].targetAngles[3] += d * 0.1 * np.pi if (state == 0): self.states[state].setTargetAngles(pose[state]) #self.states[state].targetAngles[0] += d * 0.03 * np.pi # self.states[state].targetAngles[4] += d * 0.01 * np.pi # self.states[state].targetAngles[5] += d * 0.01 * np.pi self.states[state].targetAngles[4] += v * 0.1 # self.states[state].targetAngles[2] += d * 0.01 * np.pi''' if (state == 2): self.states[state].setTargetAngles(pose[state]) #self.states[state].targetAngles[0] += d * 0.03 * np.pi # self.states[state].targetAngles[4] += d * 0.01 * np.pi # self.states[state].targetAngles[5] += d * 0.01 * np.pi self.states[state].targetAngles[1] += v * 0.1
def captureJointAction(self, t): x = [] y = [] y2 = [] plt.title("Matplotlib demo") plt.xlabel("x axis caption") plt.ylabel("y axis caption") for i in range(int(t // self.step_time)): x.append(i) info = bullet.getJointState(self.humanoid_id, self.joint_id['4']) y.append(info[0]) y2.append(info[1]) bullet.stepSimulation() time.sleep(self.step_time) plt.plot(x, y, "ob") plt.plot(x, y2, "or") plt.show()
def getJointRanges(self): lowerLimits, upperLimits, jointRanges, restPoses = [], [], [], [] for i in range(self.numJoints): jointInfo = p.getJointInfo(self.icubId, i) if jointInfo[3] > -1: ll, ul = jointInfo[8:10] jr = ul - ll # For simplicity, assume resting state == initial state rp = p.getJointState(self.icubId, i)[0] lowerLimits.append(ll) upperLimits.append(ul) jointRanges.append(jr) restPoses.append(rp) return lowerLimits, upperLimits, jointRanges, restPoses
def step(self): goals = copy.copy(self.joint_state_goals) self.apply_forces(goals) bullet.stepSimulation() self.sim_time += self.time_step state_msg = JointState() state_msg.position = [] state_msg.velocity = [] for joint in self.motor_joints: joint_angle, joint_velocity, _, _ = bullet.getJointState( self.robotID, joint) state_msg.position.append(joint_angle) state_msg.velocity.append(joint_velocity) state_msg.header.stamp.sec = math.floor(self.sim_time) state_msg.header.stamp.nanosec = int( (self.sim_time * 1000000000) % 1000000000) self._joint_state_pub.publish(state_msg)
def update_joint_drive(self): max_vel = 2000 timestep = 1 / 240 kd = 0.1 # 0.1 # 0.13 gain_d = timestep * kd / (1 / 240) if 'gain_d' in self.state.config: gain_d = self.state.config['gain_d'] for i in range(12): true_target = self.state.joint_target[i] + self.state.joint_offset[ i] urdf_joint_id = self.urdf_joint_indexes[i] kp = self.state.all_kp[i] if i % 3 == 2: kp *= 0.8 gain_p = timestep * timestep * kp / (0.27) # 0.003789 if 'gain_p' in self.state.config: gain_p = self.state.config['gain_p'] deadband = self.state.joint_deadband[i] data = p.getJointState(self.robotId, urdf_joint_id, physicsClientId=self.pcId) cur_pos = data[0] delta_target = cur_pos - true_target if delta_target > deadband: delta_target = deadband elif delta_target < -deadband: delta_target = -deadband fake_target = true_target + delta_target #f = 10 # 8 # abs(delta_target/deadband)*8 f = 10 * abs(delta_target / deadband) self.state.fake_target[i] = fake_target p.setJointMotorControl2(self.robotId, urdf_joint_id, p.POSITION_CONTROL, targetPosition=fake_target, force=f, maxVelocity=max_vel, positionGain=gain_p, velocityGain=gain_d, physicsClientId=self.pcId) if self.debug: self.to_plot[i + 40].append(data[0]) self.to_plot[i + 40 + 12].append(data[3])
def get_reward(self, ee_state, pizza_state, action): ee_pos = np.array(ee_state[4]) ee_orn = ee_state[5] pizza_pos, pizza_orn = pizza_state[0] pizza_pos = np.array(pizza_pos) pizza_linear_vel, pizza_angular_vel = pizza_state[1] pan_reward = -np.linalg.norm(ee_orn - pan_default_orn)**2 catch_rew = -0.1 * np.linalg.norm(ee_pos - pizza_pos)**2 catch_rew += np.clip(pizza_pos[2] - ee_pos[2], -np.inf, 0.) flip_rew = -np.linalg.norm(target_flip_orn - pizza_orn)**2 if flip_rew > self._max_flip_reward: self._max_flip_reward = flip_rew jnt_reward = 0. for ctrl_idx in jnt_ctrl_idx: th, thdot, thFx, thJx = bullet_client.getJointState( self.robot_id, ctrl_idx) jnt_reward += -(th - rp[ctrl_idx])**2 - 0.001 * (thdot)**2 return catch_rew + flip_rew + jnt_reward
def movj_speed_control(self, targj, speed=7.0): currj = [p.getJointState(self.ur5, i)[0] for i in self.joints] currj = np.array(currj) diffj = targj - currj if all(np.abs(diffj) < 0.1): return False norm = np.linalg.norm(diffj) v = diffj / norm if norm > 0 else 0 stepj = diffj * speed gains = np.ones(len(self.joints)) p.setJointMotorControlArray(bodyIndex=self.ur5, jointIndices=self.joints, controlMode=p.VELOCITY_CONTROL, targetVelocities=stepj, velocityGains=gains) p.stepSimulation()
def observe_state(self): force_feedback = p.getJointState(self.pw.robot, self.finger_joints[0])[ 2] #should be symmetric across fingers, hence picking one binary_threshold = 1.5 #arbitrary really binary_force_feedback = (np.abs(force_feedback) > binary_threshold).astype(np.int32) if self.force_feedback_type == None: return self.get_pos() elif self.force_feedback_type == "cont": return np.hstack([self.get_pos(), force_feedback]).flatten() elif self.force_feedback_type == "binary": return np.hstack([self.get_pos(), binary_force_feedback]).flatten() elif self.force_feedback_type == "discrete": discrete_force_feedback = np.array(force_feedback).astype(np.int32) return np.hstack([self.get_pos(), discrete_force_feedback]).flatten() else: return self.get_pos()
def getJointState(self): position_list = [] velocity_list = [] reactionForce_list = [] torque_list = [] for i in self.joint_id_list: position, velocity, reactionForce, torque = p.getJointState( self.robotId, i) position_list.append(position) velocity_list.append(velocity) reactionForce_list.append(reactionForce) torque_list.append(torque) position_list = np.array(position_list) velocity_list = np.array(velocity_list) torque_list = np.array(torque_list) reactionForce_list = np.array(reactionForce_list) return position_list, velocity_list, torque_list, reactionForce_list
def get_joint_ranges(self, includeFixed=False): all_joints = pyplan.get_movable_joints(self.id) upper_limits = pyplan.get_max_limits(self.id, all_joints) lower_limits = pyplan.get_min_limits(self.id, all_joints) jointRanges, restPoses = [], [] numJoints = p.getNumJoints(self.id) for i in range(numJoints): jointInfo = p.getJointInfo(self.id, i) if includeFixed or jointInfo[3] > -1: rp = p.getJointState(self.id, i)[0] jointRanges.append(2) restPoses.append(rp) return lower_limits, upper_limits, jointRanges, restPoses
def setRobot(self, robot, physicsClientId=None): self._robot = robot if physicsClientId != None: self._physicsClientId = physicsClientId joint_id_list = [] joint_pos_list = [] self._joint_number = 0 for i in range(p.getNumJoints(robot)): jointInfo = p.getJointInfo(robot, i) if jointInfo[2] == 0: joint_id_list.append(jointInfo[0]) joint_pos_list.append(p.getJointState(robot, jointInfo[0])[0]) self._joint_number += 1 print(self._joint_number) self._joint_id = np.array(joint_id_list, dtype=np.int32) self._joint_targetPos = np.array(joint_pos_list, dtype=np.float) self._joint_currentPos = np.array(joint_pos_list, dtype=np.float)
def set_joint_velocity(self, joint_uid, velocity): """Set the position of the joint. The joint position will be set to the current position. Args: joint_uid: A tuple of the body Unique ID and the joint index. vel: A float32 value. """ body_uid, joint_ind = joint_uid position, _, _, _ = pybullet.getJointState(bodyUniqueId=body_uid, jointIndex=joint_ind, physicsClientId=self.uid) pybullet.resetJointState(bodyUniqueId=body_uid, jointIndex=joint_ind, targetValue=position, targetVelocity=velocity, physicsClientId=self.uid)
def auto_close_gripper(self, step: int = 120, check_contact: bool = False) -> bool: # Get initial gripper open position initial_position = p.getJointState( self.robot_id, self.joints[self.mimicParentName].id)[0] initial_position = math.sin(0.715 - initial_position) * 0.1143 + 0.010 for step_idx in range(1, step): current_target_open_length = initial_position - step_idx / step * initial_position self.move_gripper(current_target_open_length, 1) if current_target_open_length < 1e-5: return False # time.sleep(1 / 120) if check_contact and self.gripper_contact(): return True return False
def step_jointspace(self, actions): """ Executes a step in taskspace providing endeffector deltas dx, dy, dz and endeffector rotation -------- Parameters: actions: list(dx, dy, dz, dtheta) """ joint_pos = [ p.getJointState(self.o.kukaobject.kukaId, i)[0] for i in range(p.getNumJoints(self.o.kukaobject.kukaId)) ] link_state = p.getLinkState(self.o.kukaobject.kukaId, self.o.kukaobject.kukaEndEffectorIndex) curr_eepos = link_state[0] curr_orn = link_state[1] tgt_eepos = list_add(list(curr_eepos), actions[:3]) tgt_rot = joint_pos[7] + actions[-1] curr_euler_angle = p.getEulerFromQuaternion(curr_orn) tgt_orn = list_add( curr_orn, p.getQuaternionFromEuler( list_add(curr_euler_angle, [actions[-2], actions[-3], actions[-1]]))) for i in range(self._numJoints): p.setJointMotorControl2(bodyIndex=self.o.kukaobject.kukaId, jointIndex=i, controlMode=p.POSITION_CONTROL, targetPosition=joint_pos[i] + actions[i], targetVelocity=0, force=200, positionGain=0.01, velocityGain=0.35) self.o.kukaobject.gripper_stabler_keep_commands() for kuka_sec in range(1): p.stepSimulation() #if self.render: if True: time.sleep(0.001)
def showBestStanding(self, num=-1, log=0): self.Disconnect() self.RunSRV(1) time.sleep(1) '''print("\nInShow, {:.3f}".format(self.elites[-1][0][0]))''' #angles = np.array([90-38+30, 180-83, 180-110, 63, 90-38+30, -(180-83), -(180-110), -63])*np.pi/180#self.population[sort[0]][0] #botStartOrientation = pb.getQuaternionFromEuler([0,-30*np.pi/180,0]) #angles = np.array([90-38, 180-83, 180-110, 63, 90-38, -(180-83), -(180-110), -63])*np.pi/180 #botStartOrientation = pb.getQuaternionFromEuler([0,0,0]) #sort = np.argsort(-self.fitnesses) #angles = self.population[sort[0]][0] #botStartOrientation = pb.getQuaternionFromEuler([0, self.population[sort[0]][1], 0]) angles = self.elites[num][0] botStartOrientation = pb.getQuaternionFromEuler( [0, self.elites[num][1], 0]) cid, botId = self.Init(botStartOrientation, pb_type=pb.SHARED_MEMORY) if log == 1: logID = pb.startStateLogging( loggingType=pb.STATE_LOGGING_VIDEO_MP4, fileName="bestStanding.mp4") print([cid, botId]) self.setLegs(angles, sleep=0, botID=botId, cid=cid) self.Torques = [] for dur in range(0, 1000): self.Step(1, sleep=1, cid=cid, botID=botId) self.AdjustCamera(botID=botId, cid=cid) pb.setJointMotorControlArray(botId, range(8), controlMode=pb.POSITION_CONTROL, targetPositions=angles, targetVelocities=[0] * 8, forces=[99999999999] * 8) torques = [] for j in range(8): tmp, tmp, tmp, t = pb.getJointState(botId, j) torques.append(t) self.Torques.append(torques) if log == 1: pb.stopStateLogging(logID)
def _get_joint_list_info_and_state_as_dict(self): jointType = { 0: "JOINT_REVOLUTE", 1: "JOINT_PRISMATIC", 2: "JOINT_SPHERICAL", 3: "JOINT_PLANAR", 4: "JOINT_FIXED" } #initialize jointList which is a list of dictionaries for every joint lstJointInfoAndState = [] #get total number of joints in robot id n_joints = p.getNumJoints(self.arm) print("Total number of joints is ", n_joints) for i in range(0, n_joints): #get current joint currJointInfo = p.getJointInfo(self.arm, i) currJointState = p.getJointState(self.arm, i) #write current joint info to dictionary currJointInfo = { "jointIndex": currJointInfo[0], "jointName": currJointInfo[1], "jointType": jointType[currJointInfo[2]], "qIndex": currJointInfo[3], "uIndex": currJointInfo[4], "jointLowerLimit": currJointInfo[8], "jointUpperLimit": currJointInfo[9], "jointAxis": currJointInfo[13], "parentFramePos": currJointInfo[14], "parentIndex": currJointInfo[15] } currJointState = { "jointPosition": currJointState[0], "jointVelocity": currJointState[1], "jointReactionForces": currJointState[2], "appliedJointMotorTorque": currJointState[3] } currJointInfoAndState = currJointInfo.copy() currJointInfoAndState.update(currJointState) lstJointInfoAndState.append(currJointInfoAndState) return lstJointInfoAndState
def get_joint_reaction_force(self, joint_uid): """Get the reaction force of the joint. These are the joint reaction forces, if a torque sensor is enabled for this joint it is [Fx, Fy, Fz, Mx, My, Mz]. Without torque sensor, it is [0, 0, 0, 0, 0, 0]. Args: joint_uid: A tuple of the body Unique ID and the joint index. Returns: A 3-dimensional float32 numpy array. """ body_uid, joint_ind = joint_uid _, _, reaction_force, _ = pybullet.getJointState( bodyUniqueId=body_uid, jointIndex=joint_ind, physicsClientId=self.uid) return np.array(reaction_force, dtype=np.float32)
for i in range(numLines): spacing = 0.01 textPos = [.1 - (i + 1) * spacing, .1, 0.011] text = "Frame:" + str(frameNr) + "\nObject UID:" + objectInfo textUid = p.addUserDebugText(text, textColorRGB=[0, 0, 0], textSize=0.02, textPosition=textPos, textOrientation=textOrn, parentObjectUniqueId=uiCube, parentLinkIndex=-1, replaceItemUniqueId=lines[i]) lines[i] = textUid #keep the gripper centered/symmetric b = p.getJointState(pr2_gripper, 2)[0] p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) events = p.getVREvents() for e in (events): if e[CONTROLLER_ID] == uiControllerId: p.resetBasePositionAndOrientation(uiCube, e[POSITION], e[ORIENTATION]) pos = e[POSITION] orn = e[ORIENTATION] lineFrom = pos mat = p.getMatrixFromQuaternion(orn) dir = [mat[0], mat[3], mat[6]] to = [pos[0] + dir[0] * rayLen, pos[1] + dir[1] * rayLen, pos[2] + dir[2] * rayLen] hit = p.rayTest(lineFrom, to) oldRay = pointRay color = [1, 1, 0]
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.) 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[j])/jointDirections[j])) p.setRealTimeSimulation(1) while (1): for i in range(len(paramIds)): c = paramIds[i] targetPos = p.readUserDebugParameter(c) maxForce = p.readUserDebugParameter(maxForceId)
import pybullet as p import time p.connect(p.GUI) cartpole=p.loadURDF("cartpole.urdf") p.setRealTimeSimulation(1) p.setJointMotorControl2(cartpole,1,p.POSITION_CONTROL,targetPosition=1000,targetVelocity=0,force=1000, positionGain=1, velocityGain=0, maxVelocity=0.5) while (1): p.setGravity(0,0,-10) js = p.getJointState(cartpole,1) print("position=",js[0],"velocity=",js[1]) time.sleep(0.01)
targetVelocity=0,force=500, positionGain=1,velocityGain=0.1) #Tests that the robot can match the simulation position #TODO: Move commands to different node so this doesn't slow down so much rate = rospy.Rate(100) while True: msg = uc_msg() p.stepSimulation() i = 0 l_conf = [] r_conf = [] for joint in range(len(left_joints)): l_conf.append(p.getJointState(pr2, left_joints[joint])[0]) for joint in range(len(right_joints)): r_conf.append(p.getJointState(pr2, right_joints[joint])[0]) msg.l_shoulder_pan_joint = l_conf[0] msg.l_shoulder_lift_joint = l_conf[1] msg.l_upper_arm_roll_joint = l_conf[2] msg.l_elbow_flex_joint = l_conf[3] msg.l_forearm_roll_joint = l_conf[4] msg.l_wrist_flex_joint = l_conf[5] msg.l_wrist_roll_joint = l_conf[6] msg.r_shoulder_pan_joint = r_conf[0] msg.r_shoulder_lift_joint = r_conf[1] msg.r_upper_arm_roll_joint = r_conf[2]
print("loaded!") #p.changeDynamics(sphere ,-1, mass=1000) door = p.loadURDF("door.urdf",[0,0,-11]) p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1) print("numJoints = ", p.getNumJoints(door)) p.setGravity(0,0,-10) position_control = True angle = math.pi*0.25 p.resetJointState(door,1,angle) angleread = p.getJointState(door,1) print("angleread = ",angleread) prevTime = time.time() angle = math.pi*0.5 count=0 while (1): count+=1 if (count==12): p.stopStateLogging(logId) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) curTime = time.time() diff = curTime - prevTime
minitaur = p.loadURDF("quadruped/minitaur_single_motor.urdf", useFixedBase=True) print(p.getNumJoints(minitaur)) p.resetDebugVisualizerCamera(cameraDistance=1, cameraYaw=23.2, cameraPitch=-6.6, cameraTargetPosition=[-0.064, .621, -0.2]) motorJointId = 1 p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=100000, force=0) p.resetJointState(minitaur, motorJointId, targetValue=0, targetVelocity=1) angularDampingSlider = p.addUserDebugParameter("angularDamping", 0, 1, 0) jointFrictionForceSlider = p.addUserDebugParameter("jointFrictionForce", 0, 0.1, 0) textId = p.addUserDebugText("jointVelocity=0", [0, 0, -0.2]) p.setRealTimeSimulation(1) while (1): frictionForce = p.readUserDebugParameter(jointFrictionForceSlider) angularDamping = p.readUserDebugParameter(angularDampingSlider) p.setJointMotorControl2(minitaur, motorJointId, p.VELOCITY_CONTROL, targetVelocity=0, force=frictionForce) p.changeDynamics(minitaur, motorJointId, linearDamping=0, angularDamping=angularDamping) time.sleep(0.01) txt = "jointVelocity=" + str(p.getJointState(minitaur, motorJointId)[1]) prevTextId = textId textId = p.addUserDebugText(txt, [0, 0, -0.2]) p.removeUserDebugItem(prevTextId)
import pybullet as p p.connect(p.DIRECT) hinge = p.loadURDF("hinge.urdf") print("mass of linkA = 1kg, linkB = 1kg, total mass = 2kg") hingeJointIndex = 0 #by default, joint motors are enabled, maintaining zero velocity p.setJointMotorControl2(hinge, hingeJointIndex, p.VELOCITY_CONTROL, 0, 0, 0) p.setGravity(0, 0, -10) p.stepSimulation() print("joint state without sensor") print(p.getJointState(0, 0)) p.enableJointForceTorqueSensor(hinge, hingeJointIndex) p.stepSimulation() print("joint state with force/torque sensor, gravity [0,0,-10]") print(p.getJointState(0, 0)) p.setGravity(0, 0, 0) p.stepSimulation() print("joint state with force/torque sensor, no gravity") print(p.getJointState(0, 0)) p.disconnect()
def get_state(self): x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex) return x, vx