def step(self, action): print("the action taken is") print(action) print(type(action)) #action["decide which joints to move"] print(action) #create assertions for action space assert action["decide which joints to move"] in list[range(0,self.number_of_joints)] #, action["decide which joints to move"] # decide what joints to lock and which to leave free list_index = action["decide which joints to move"] self.list_of_combinations[ list_index ] #choose which cobination of joints to unlock (represented as string) for i in self.list_of_combinations[ list_index ]: print(action["decide which joints to move"]) #sdf # perform actation of joints (create for loop to step through simulation) (input a torque on joints for certain amount of loops) #if action == 0 and self.cur_pos > 0: # self.cur_pos -= 1 #elif action == 1: # self.cur_pos += 1 #done = self.distance_to_goal >= self.end_pos #GET END EFFECTOR POSITION #side note : base position is (0,0,2) ( ( l1, l2, l3, l4, l5, l6 ), ) = p.getLinkStates(bodyUniqueId=plane, linkIndices=[self.total_number_of_links - 1] ) end_effector_world_position = np.asarray([l1]) #convert tuple to numpy array (so that we can compute euclidean distance later using scipy) distance_to_goal_vector = self.desired_xyz_position_of_end_effoctor - self.current_xyz_position_of_end_effector print("distance to goal vector is:") print( distance_to_goal_vector ) observation["distance_to_goal"], observation["angle_theta_to_goal"], observation["angle_phi_to_goal"] = einsteinpy.utils.coord_transforms.CartesianToSpherical_pos( distance_to_goal_vector ) #GET JOIN done = distance_to_goal <= self.distance_to_goal_threshold #return [self.cur_pos], 1 if done else 0, done, {} if self.time_to_reach_goal > 5 self.distance_to_goal < self.distance_to_goal_epsilon: done = True
def observeState(self): if self.SIM_READDY == 1: self.getObservedLink() ss_tmp = p.getLinkStates(self.body_id, self.observed_index, 1, physicsClientId=self.client_id) self.states = [] self.states_dot = [] self.states_sincos = [] for i in range(len(self.observed_index)): self.states.append( ss_tmp[i][0] + p.getEulerFromQuaternion(ss_tmp[i][1]) ) ##[ [pos,pos,pos,ang,ang,ang] , [pos,pos,pos,ang,ang,ang] , ... ,for each Link] if self.SINCOS == 1: self.states_sincos.append(( np.sin(self.states[i][3]), np.cos( self.states[i][3] ), # [ [sin,cos,sin,cos,sin,cos] ,..., for each Link] np.sin(self.states[i][4]), np.cos(self.states[i][4]), np.sin(self.states[i][5]), np.cos(self.states[i][5]))) self.states_dot.append( ss_tmp[i][6] + ss_tmp[i][7] ) ##[ [vel,vel,vel,omega,omega,omega] ,...., for each link] #for logging if self.LOGDATA == 1: self.state_seq.append(self.states) self.state_dot_seq.append(self.states_dot) else: raise Exception("Simulation not set up")
def get_obs(self): velocity = p.getBaseVelocity(self.bot_id) all_links = [num for num in range(6)] link_states = p.getLinkStates(self.bot_id, all_links) obs = [] for kk in range(len(all_links)): obs.extend(link_states[kk][0]) obs.extend(link_states[kk][1]) obs.extend(velocity[0]) obs.extend(velocity[1]) obs.append(self.fuel) obs = np.array(obs) # block_state = p.getLinkState(bodyUniqueId=self.bot_id,\ # linkIndex=1) # # # fov, aspect, nearplane, farplane = 60, 1.0, 0.01, 100 # projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, nearplane, farplane) # # Center of mass position and orientation (of link-7) # com_p, com_o, _, _, _, _ = p.getLinkState(self.bot_id, 0, \ # computeForwardKinematics=True) # com_p, com_o = cube_position, cube_orientation # rot_matrix = p.getMatrixFromQuaternion(com_o) # rot_matrix = np.array(rot_matrix).reshape(3, 3) # # Initial vectors # init_camera_vector = np.array((0, 1, 0)) # z-axis # init_up_vector = (0, 0, 1) # y-axis # # Rotated vectors # camera_vector = rot_matrix.dot(init_camera_vector) # up_vector = rot_matrix.dot(init_up_vector) # view_matrix = p.computeViewMatrix(com_p, com_p + 0.1 * camera_vector, up_vector) # view_image = p.getCameraImage(self.dim_x, self.dim_y, view_matrix, projection_matrix) return obs
def __init__(self, config): #connect to pybullet ohysics server if config["env_render"] == 1: clientId = p.connect(p.GUI) else: clientId = p.connect(p.DIRECT) #clientId = p.connect(p.GUI) #clears all urdf (Unified Robot Description Format) aka the 3D model of our robot p.resetSimulation() #set gravity p.setGravity(0,0,-10) #determine if our simulation will: (1) run in real time or if, (0) we need to manually step (a fixed timestep) when we take a atep in out RL gym envorionment useRealTimeSim = 1 p.setRealTimeSimulation(useRealTimeSim) #find directory of file where our urdf is located print( os.path.join( pybullet_data.getDataPath(), "romans_urdf_files/octopus_files/python_scripts_edit_urdf/output2.urdf" ) ) #load our urdf plane = p.loadURDF( os.path.join( pybullet_data.getDataPath(), "romans_urdf_files/octopus_files/python_scripts_edit_urdf/output2.urdf" ) ) #octobot hyperparameters self.total_number_of_links = config["total_number_of_links"] self.number_of_free_joints = 1 # config["number_of_free_joints"] self.total_number_of_joints = self.total_number_of_links #torque limits (originally same for each joint but we can change that later) self.torque_limit_min = -1000 self.torque_limit_max = 1000 #joint state limits (originally same for each joint (also same for joint positions and velocities) but we can change that later) self.joint_states_min = 1000 self.joint_states_max = 1000 #initial distance to goal vector (in polar coordinates) aka randomaly assign where we want our end effector to move to self.distance_to_goal_epsilon = 0.5 #how close (in radial distance) does the end effector need to be to the goal in order for us to consider the goal to be "reached" self.max_distance_to_goal = 8 # trying to reach far goals will take a long time to obtain rewards self.max_radial_distance_from_base = 8 # helps make sure goal is within reachable workspace # Sample goal points (for case of 2D arm) #For reference "sample points uniformly within a circle" #https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly # takeaway: account for the fact that radius doubles witin a sphere R = self.max_radial_distance_from_base_to_goal self.radial_distance_from_base_to_goal = R * math.sqrt( random.random() ) # radial distance from base to goal self.angle_from_base_to_goal_theta = random.random() * 2 * math.pi #base to goal theta centerX = 0 #in pybullet, this is actually th y coordinate centerY = 2 #in pybullet, this is actually the z coordinate x = centerX + r * math.cos(self.angle_from_base_to_goal_theta) y = centerY + r * math.sin(self.angle_from_base_to_goal_theta) #theta is given for 2D case but not relevant input to NN #base to goal phi if x>=0: self.angle_from_base_to_goal_phi = math.pi/2 # +y cartesian coord gives +theta, -y gives -theta else: self.angle_from_base_to_goal_phi = -math.pi/2 #Sample points (for case of 3D arm) #sample points uniformly within a sphere #https://stackoverflow.com/a/5408843 # takeaway: account for the fact that volume #phi = random.uniform(0,2pi) #costheta = random.uniform(-1,1) #u = random.uniform(0,1) #theta = arccos( costheta ) #r = R * cuberoot( u ) #x = r * sin( theta) * cos( phi ) #y = r * sin( theta) * sin( phi ) #z = r * cos( theta ) #self.distance_to_goal = random.uniform( 0, self.distance_to_goal_threshold ) # #self.angle_to_goal_theta = random.uniform(0, 2*3.14158 ) #elevation angle #self.angle_to_goal_phi = random.uniform(0, 2*3.14158 ) #azimuthal angle #self.radial_distance_from_base_to_goal = r #random.uniform( 0, self.max_radial_diastance_from_base ) #self.angle_from_base_to_goal_theta = theta #random.uniform(0, 2*3.14158 ) #elevation angle #self.angle_from_base_to_goal_phi = (math.pi)/2 #2d arm case #random.uniform(0, 2*3.14158 ) #azimuthal angle #get xyz potition of goal self.cartesianPositionOfGoal = [0, x, y] #get cartesian vector from base to EE (end effector) ( ( l1, l2, l3, l4, l5, l6 ), ) = p.getLinkStates(bodyUniqueId=plane, linkIndices=[ self.total_number_of_links - 1 ] ) #returns tuple self.vector_from_base_to_EE_cartesian = list(l1) #vector from base to EE #get cartesian vector from EE to goal self.vector_from_EE_to_goal_cartesian = np.subtract( self.vector_from_base_to_goal, self.vector_from_base_to_EE ) #returns nupy array #get disance from EE to goal #self.radial_distance_from_EE_to_goal = np.linalg.norm(self.vector_from_EE_to_goal) # returns numpy float.64 #random.uniform( 0, self.distance_to_goal_threshold ) #get polar vector from EE to goal self.vector_from_EE_to_goal_polar = coordinates.utils.cartesian_to_spherical_novel( self.vector_from_EE_to_goal_cartesian[0] , self.vector_from_EE_to_goal_cartesian[1] , self.vector_from_EE_to_goal_cartesian[2] ) # extract each polar coordinates (r, theta, phi) from EE_to_goal_vector to gym environment self.radial_distance_from_base_to_goal = self.vector_from_EE_to_goal_polar[0] #random.uniform( 0, self.max_radial_diastance_from_base ) self.angle_from_base_to_goal_theta = self.vector_from_EE_to_goal_polar[1] #random.uniform(0, 2*3.14158 ) #elevation angle self.angle_from_base_to_goal_phi = self.vector_from_EE_to_goal_polar[2] #random.uniform(0, 2*3.14158 ) #azimuthal angle #turn spherical position of goal to cartesian coordinates (for visualization) #self.position_of_goal = einsteinpy.utils.coord_transforms.SphericalToCartesian_pos( self.radial_distance_from_base_to_goal, self.angle_from_base_to_goal_theta, self.angle_from_base_to_goal_phi ) self.vector_from_base_to_goal_cartesian = coordinates.utils.spherical_to_cartesian_novel( self.radial_distance_from_base_to_goal, self.angle_from_base_to_goal_theta, self.angle_from_base_to_goal_phi ) #self.distance_to_goal = random.uniform( 0, self.distance_to_goal_threshold ) # #self.angle_to_goal_theta = random.uniform(0, 2*3.14158 ) #elevation angle #self.angle_to_goal_phi = random.uniform(0, 2*3.14158 ) #azimuthal angle # This function generates all binary strings of length n with k number of bits(... akak k number of ones) # (example: n=3, k=2, kbits(n,k) gnerates ['110', '101', '011']) # https://stackoverflow.com/questions/1851134/generate-all-binary-strings-of-length-n-with-k-bits-sets ### import itertools def kbits(n, k): result = [] for bits in itertools.combinations(range(n), k): s = ['0'] * n for bit in bits: s[bit] = '1' result.append(''.join(s)) return result #use off the shelf kbits function to generate all possible list of combinations (for free joints) self.list_of_combinations = kbits(self.total_number_of_links, self.number_of_free_joints) #CREATE OBSERVATION SPACE #how to use Box object type #examples of how to use Box are found in Documents/RoboLab/roman_playful/gym/gym/spaces/box.py self.observation_space = Dict( { "radial distance to goal" : Box( low = 0, high = self.distance_to_goal_threshold, dtype = np.floatt32 ) , "angle (polar,theta) to goal" : Box( low = 0, high = 2*3.15, dtype = np.float32 ) , "angle (azimuthal) to goal" : Box( low = 0, high - 2*3.15, dtype = np.float32 ), "joint states" : Box( low = self.joint_states_min*np.ones( self.total_number_of_joints), high = self.joint_states_max*np.ones(self.total_number_of_joints) , dtype=np.float32 ) , } ) #CREATE ACTION SPACE #how to use Discrete object type #Documents/RoboLab/roman_playful/gym/gym/spaces/discrete.py self.action_space = Dict({ "decide which joints to move" : Discrete( self.number_of_free_joints ) , #chose an index that specifies which binary string will be used to decide whoch joints are locked and which joints are free "decide torques on each free joint" : Box( low = self.torque_limit_min*np.ones( self.number_of_free_joints), high = self.torque_limit_max*np.ones(self.number_of_free_joints) , dtype=np.float32 ) , #decided torques to implement on each free joint })
import math _client = bullet_client.BulletClient(connection_mode=p.GUI) _client.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 1) _client.setAdditionalSearchPath("./data") #_client.setAdditionalSearchPath(pybullet_data.getDataPath()) z2y = p.getQuaternionFromEuler([-math.pi * 0.5, 0, 0]) robotId = _client.loadURDF( "big_test.urdf", [0, 0, 0], #globalScaling=0.25 ) num_joints = p.getNumJoints(robotId) jointIndices = range(num_joints) while True: _client.stepSimulation() simJointStates = p.getJointStatesMultiDof(robotId, jointIndices) print("joint states", simJointStates) link_states = p.getLinkStates(robotId, jointIndices) print("link states") for i, a in enumerate(link_states): print("com: ", a[0]) info = p.getJointInfo(robotId, i) print(info) #break
def collectObservations(self): ''' Collect state information on humanoid joints. Output of getJointStates() will be in the following format: 0 - position: float 1 - velocity: float 2 - Joint Reaction Forces: list of 6 floats 3 - applied Joint Motor Torque: fload (does not add more information if we use torque control since we know the input torque) getJointStatesMultiDof() is used for Spherical Joints and returns 3 dimensional outputs instead of 1D output for revolute joints ''' # Collect the base position, orientation and velocity of the "root" of the agent rootPosition, rootOrientation = p.getBasePositionAndOrientation(self.humanoidAgent) rootVelocity, rootAngularVelocity = p.getBaseVelocity(self.humanoidAgent) # Make calls to Pybullet to collect joint information revoluteJointStates = p.getJointStates( self.humanoidAgent, jointIndices=self.revoluteJoints, ) sphericalJointStates = p.getJointStatesMultiDof( self.humanoidAgent, jointIndex=self.sphericalJoints, ) # Collect the position of end effectors (Hands and Feet) endEffectorPositions = [position[0] for position in p.getLinkStates(bodyUniqueId=self.get_ids(), linkIndices=[5, 8, 11, 14])] endEffectorPositionsFlattened = [position for link in endEffectorPositions for position in link] # use the first two values from each joint state (position, velocity) and flatten all tuples into a list # output is a flattened list containing: # 1D position and velocity for revolute Joints, followed by # Quaternion rotation and 3d angular velocity vectors for spherical Joints rootState = [position for position in rootPosition] + \ [orientation for orientation in rootOrientation] + \ [velocity for velocity in rootVelocity] + \ [angularVelocity for angularVelocity in rootAngularVelocity] ''' output index structure of allJointStates: [0, 1, 2] - root position (xyz) [3, 4, 5, 6] - root orientation quaternion (xyzw) [7, 8, 9] - root velocity (xyz) [10, 11, 12] - root angular velocity (xyz) [13-14, 15-16, 17-18, 19-20] - (1D position, 1D velocity) for each revolute joint in the order: rKnee, rElbow, lKnee, lElbow [21-27, 28-34...70-76] - (4D orientation Quaternion, 3D angular velocity) for each spherical joint in order: chest, neck, rHip, rAnkle, rShoulder, lHip, lAnkle, lShoulder [77-79, 80-82...86-88] - 3D global positions of End Effectors (hands and feet of agent): rAnkle, rWrist, lAnkle, lWrist ''' allJointStates = rootState + \ [jointInfo for joint in revoluteJointStates for jointInfo in joint[:2]] + \ [jointInfo for joint in sphericalJointStates for jointInfo in list(sum(joint[:2], ()))] + \ endEffectorPositionsFlattened return allJointStates
def step(self, ctrl): ''' Step function. :param ctrl: list or array of target joint angles normalized to [-1,1] :return: next_obs, r, done, _ . The last '_' value is left as compatability with gym envs. ''' # YOU CAN REMOVE THIS CLIP IF YOU WANT ctrl_clipped = np.clip(ctrl, -1, 1) # Scale the action to correct range and publish to simulator (this just publishes the action, doesn't step the simulation yet) scaled_action = self.norm_to_rads(ctrl_clipped) p.setJointMotorControlArray(bodyUniqueId=self.robot, jointIndices=self.joints_index, controlMode=p.POSITION_CONTROL, targetPositions=scaled_action, forces=self.max_forces, positionGains=[0.02] * self.act_dim, velocityGains=[0.1] * self.act_dim, physicsClientId=self.client_ID) # Step the simulation. p.stepSimulation(physicsClientId=self.client_ID) if self.animate: #time.sleep(0.004) time.sleep(.005) # Get new observations (Note, not all of these are required and used) torso_pos, torso_quat, torso_vel, torso_angular_vel, joint_angles, joint_velocities, joint_torques, contacts = self.get_obs( ) xd, yd, zd = torso_vel # Unpack for clarity roll, pitch, yaw = p.getEulerFromQuaternion(torso_quat) # get positions of all links tmp = p.getLinkStates(self.robot, range(0, 29), physicsClientId=self.client_ID) pos = [] for link in tmp: pos += [link[0]] joint_angles_n = ((joint_angles - self.joints_rad_low) / self.joints_rad_diff) * 2 - 1 self.step_ctr += 1 """reward """ r_foot = np.array( p.getLinkState(self.robot, self.right_foot, physicsClientId=self.client_ID)[0]) l_foot = np.array( p.getLinkState(self.robot, self.left_foot, physicsClientId=self.client_ID)[0]) dist_r_foot = np.linalg.norm(r_foot - self.r_foot_target) dist_l_foot = np.linalg.norm(l_foot - self.l_foot_target) reward_r_foot = 1 / (dist_r_foot + 0.2) # it gives form 0 to 5 reward_l_foot = 1 / (dist_l_foot + 0.2) # it gives from 0 to 5 reward_r_arm_down = 1 / (abs( p.getJointState(self.robot, 20, physicsClientId=self.client_ID)[0]) **2 + 0.2) # it gives from 0 to 5 reward_l_arm_down = 1 / (abs( p.getJointState(self.robot, 25, physicsClientId=self.client_ID)[0]) **2 + 0.2) # it gives from 0 to 5 reward_r_knee = 1 / (abs( p.getJointState(self.robot, 9, physicsClientId=self.client_ID)[0]) + 0.2) # it gives from 0 to 5 reward_l_knee = 1 / (abs( p.getJointState(self.robot, 16, physicsClientId=self.client_ID)[0]) + 0.2) # it gives from 0 to 5 punish_vel_torso = 1 / (np.linalg.norm(torso_vel)**4 + 0.2) - 5 # it gives from -5 to 0 punish_bad_angle_torso = (abs(roll) / pi) * (-2.5) + ( abs(pitch) / pi) * (-2.5) # it gives from -5 to 0 r = (reward_l_foot + reward_r_foot + reward_l_arm_down + reward_r_arm_down + reward_l_knee + reward_r_knee) / 6.0 + ( punish_vel_torso + punish_bad_angle_torso) / 2.0 """reward """ # get joint_angles into np array and make observation env_obs = np.concatenate( (torso_pos, [roll, pitch, yaw], joint_angles_n, joint_velocities, contacts)).astype(np.float32) # This condition terminates the episode - WARNING - it can cause that the robot will try # terminate the episode as quickly as possible #done = self.step_ctr > self.max_steps or (legs_rot <.2 and abs(yaw-pi/2) < .2) or (np.abs(roll) > 1.5 or np.abs(pitch) > 1.5) or torso_z < 1 done = self.step_ctr > self.max_steps or torso_pos[2] < 0.4 or r > 4.2 # TODO: why random element? env_obs_noisy = env_obs # + np.random.rand(self.obs_dim).astype(np.float32) * 0.1 - 0.05 return env_obs_noisy, r, done, {}
def follow_path(id): X = [] Y = [] Z = [] kukaBasePosition = p.getLinkStates(kukaId, [0])[0][0] kukaBaseOrientation = p.getEulerFromQuaternion( p.getLinkStates(husky, [0])[0][1]) pts = get_path(id) (py, pz) = pts[0] px = 0.7 q1, q2, q3, q4, q5, q6 = get_kuka_angles(kukaBasePosition, kukaBaseOrientation, [px, py, pz], [0, 0, 0]) p.setJointMotorControlArray(kukaId, range(11), p.POSITION_CONTROL, targetPositions=[ q1, q2, q3, q4, q5, q6, curr_joint_value[6], curr_joint_value[7], curr_joint_value[8], curr_joint_value[9], curr_joint_value[10] ]) time.sleep(1) prevPose1 = [px, py, pz] hasPrevPose = 0 trailDuration = 20 t = 0 # parametric variable fig, ax = create_plot() for i in range(len(pts)): (py, pz) = pts[i] px = 0.7 # print(px, py, pz) X.append(px) Y.append(py) Z.append(pz) # getting values of each joints according to points q1, q2, q3, q4, q5, q6 = get_kuka_angles(kukaBasePosition, kukaBaseOrientation, [px, py, pz], [0, 0, 0]) p.setJointMotorControlArray( kukaId, range(11), p.POSITION_CONTROL, targetPositions=[ q1, q2, q3, q4, q5, q6, curr_joint_value[6], curr_joint_value[7], curr_joint_value[8], curr_joint_value[9], curr_joint_value[10] ]) t += 1 ls = p.getLinkState(kukaId, 5) if (hasPrevPose): p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 4, trailDuration) # prevPose = pos prevPose1 = ls[4] hasPrevPose = 1 time.sleep(.015) update_plot(X, Y, Z, fig, ax, 1001) plt.show()
# turning left walk_gen.start_stop_motions(d='rst', swayLength=swayLength) for _ in range(2): walk_gen.walk( d="l", swayLength=swayLength, turn="l", angle=np.pi / 8 ) # ("L" --> left, "R" --> rigth, "S" --> stright) --> when only 5 dof of leg is used walk_gen.walk( d="r", swayLength=swayLength, turn="l", angle=np.pi / 8 ) # ("l" --> left, "r" --> rigth, "s" --> stright) --> when all 6 dof of leg is used walk_gen.start_stop_motions(d='ls', swayLength=swayLength) ang = np.pi / 2 - p.getEulerFromQuaternion(p.getLinkStates(robot, [0])[0][1])[2] # print(p.getEulerFromQuaternion(p.getLinkStates(robot, [0])[0][1])[2], p.getEulerFromQuaternion(p.getLinkStates(robot, [0])[0][1])) if (ang > 0.01): print("turingn left : ", ang) walk_gen.walk( d="l", swayLength=0.000001, turn="l", angle=abs(ang) * 3 / 4 ) # ("L" --> left, "R" --> rigth, "S" --> stright) --> when only 5 dof of leg is used walk_gen.walk( d="r", swayLength=0.000001, turn="l", angle=abs(ang) * 3 / 4 ) # ("l" --> left, "r" --> rigth, "s" --> stright) --> when all 6 dof of leg is used elif (ang < -0.01): print("turingn right : ", ang) walk_gen.walk(d="r", swayLength=0.000001, turn="r", angle=abs(ang) * 3 / 4) walk_gen.walk(d="l", swayLength=0.000001, turn="r", angle=abs(ang) * 3 / 4) # going stright
def calculate_position_for_husky(x2, y2, z2): x1, y1, z1 = p.getLinkStates(husky, [0])[0][0] t = 0.7 / np.sqrt((x1-x2)**2 + (y1-y2)**2) x = x2 + t*(x1-x2) y = y2 + t*(y1-y2) return x, y, z2
for i in range(maxstep): p.stepSimulation() ## Do all our joint/link data handling before moving if i > 500: tuenover = isTurnedOver(RoboBoi) if (tuenover): print("FLIPPED") break else: for j in range(p.getNumJoints(RoboBoi)): #pool.apply_async(worker, setJoint(j,i,RoboBoi)) setJoint(j, i, RoboBoi) results_pos[j][i] = target[ j] = a[j] + b[j] * math.sin(c[j] * i + omega[j]) target1_speed, target2_speed, target3_speed, target4_speed = p.getLinkStates( RoboBoi, [0, 3, 9, 6], computeLinkVelocity=1) link0_velocity_vec = target1_speed[6] link0_velocity_mag = numpy.linalg.norm(link0_velocity_vec) link3_velocity_vec = target2_speed[6] link3_velocity_mag = numpy.linalg.norm(link3_velocity_vec) link9_velocity_vec = target3_speed[6] link9_velocity_mag = numpy.linalg.norm(link9_velocity_vec) link6_velocity_vec = target4_speed[6] link6_velocity_mag = numpy.linalg.norm(link6_velocity_vec) results_speed_1[i] = link0_velocity_mag results_speed_2[i] = link3_velocity_mag results_speed_3[i] = link9_velocity_mag
def update_feet(self): temp = p.getLinkStates(self.nao, self.feetIndex) self.feetCOM = np.vstack((temp[0][0], temp[1][0]))
def get_sensor_measurements(self): """ The measurements are: lateral error heading error forward speed turning rate pitch angle pitch rate They are computed assuming that both wheels are rolling without slipping and that the ground pitch is zero. """ # Position of each wheel link_states = p.getLinkStates(self.robot_id, self.joint_ids) pl = np.array(link_states[0][0]) pr = np.array(link_states[1][0]) pc = 0.5 * (pr + pl) # Velocity of each wheel joint_states = p.getJointStates(self.robot_id, self.joint_ids) q = np.zeros([self.num_joints]) v = np.zeros_like(q) for i in range(self.num_joints): q[i] = joint_states[i][0] v[i] = joint_states[i][1] vl = v[0] * self.wheel_radius vr = v[1] * self.wheel_radius # Lateral error lateral_error = np.sqrt(pc[0]**2 + pc[1]**2) - self.track_radius if self.turn_left: lateral_error *= -1. # Heading error a = np.array([pc[0], pc[1]]) a /= np.linalg.norm(a) ap = np.array([-a[1], a[0]]) b = (pr - pl)[0:2] if not self.turn_left: a *= -1. ap *= -1. heading_error = np.arctan2(np.dot(ap, b), np.dot(a, b)) # Forward speed and turning rate forward_speed = (vr + vl) / 2.0 turning_rate = (vr - vl) / np.linalg.norm(pr - pl) # Position, orientation, and angular velocity of chassis pos, ori = p.getBasePositionAndOrientation(self.robot_id) vel = p.getBaseVelocity(self.robot_id) R_body_in_world = np.reshape(np.array(p.getMatrixFromQuaternion(ori)), (3, 3)) w_in_world = np.reshape(np.array(vel[1]), (3, 1)) w_in_body = R_body_in_world.T @ w_in_world # Pitch angle and pitch rate pitch_angle = p.getEulerFromQuaternion(ori)[1] pitch_rate = w_in_body[1, 0] return lateral_error, heading_error, forward_speed, turning_rate, pitch_angle, pitch_rate
import pybullet as p import pybullet_data physicsClient = p.connect(p.GUI) p.setGravity(0, 0, 0) p.setAdditionalSearchPath(pybullet_data.getDataPath()) robotID = p.loadURDF("r2d2.urdf", [0, 0, 0], [0, 0, 1, 0]) # joints numJoint = p.getNumJoints(robotID) linkStates = [] for linkID in range(numJoint): linkStates.append(p.getLinkState(robotID, linkID)) linkStates_ = p.getLinkStates(robotID, list(range(numJoint))) baseVelocity = p.getBaseVelocity(robotID) cubePos, cubeOrn = p.getBasePositionAndOrientation(robotID) forceVec = [0, 0, 1] forcePos = [0, 0, 0] flag = p.LINK_FRAME linkID = 1 p.applyExternalForce(robotID, linkID, forceVec, forcePos, flag)
def get_tip_positions(self): states = pb.getLinkStates(self.robot_id, [5, 7]) return (states[0][0], states[1][0])
import sys sys.path.append('../../envs') import pybullet as pb from ergo_jr import PoppyErgoJrEnv env = PoppyErgoJrEnv(pb.POSITION_CONTROL) # from check/camera.py pb.resetDebugVisualizerCamera( 1.2000000476837158, 56.799964904785156, -22.20000648498535, (-0.6051651835441589, 0.26229506731033325, -0.24448847770690918)) # fixed tip is joint 5, moving tip is joint 7 link_indices = [5, 7] target_positions = [(.005, -.08, 0), (-.005, -.08, 0)] angles = env.inverse_kinematics(link_indices, target_positions) env.set_position(angles) ans = pb.getLinkStates(env.robot_id, [5, 7]) print(len(ans)) for a in ans: print(a) input('.') while True: env.step(angles)
def CoMs(self): # return the Center of Mass of the links in the global frame # [body, right_thigh, right_shin, right_toe, left_thign, left_shin, left_toe], body is ind 2 linkInd = np.arange(2,9) return np.array([np.array(s[0])[GP.PRISMA_AXIS] for s in p.getLinkStates(GP.robot,linkInd)])
def robot_orientations(robotID): states = p.getLinkStates(robotID, range(11)) orientations = np.array([p.getMatrixFromQuaternion(s[1]) for s in states]) return orientations[1:8]
def step(self, ctrl): ''' Step function. :param ctrl: list or array of target joint angles normalized to [-1,1] :return: next_obs, r, done, _ . The last '_' value is left as compatability with gym envs. ''' # YOU CAN REMOVE THIS CLIP IF YOU WANT ctrl_clipped = np.clip(ctrl, -1, 1) # Scale the action to correct range and publish to simulator (this just publishes the action, doesn't step the simulation yet) scaled_action = self.norm_to_rads(ctrl_clipped) p.setJointMotorControlArray(bodyUniqueId=self.robot, jointIndices=self.joints_index, controlMode=p.POSITION_CONTROL, targetPositions=scaled_action, forces=self.max_forces, positionGains=[0.02] * self.act_dim, velocityGains=[0.1] * self.act_dim, physicsClientId=self.client_ID) # Step the simulation. p.stepSimulation(physicsClientId=self.client_ID) if self.animate: #time.sleep(0.004) time.sleep(.005) # Get new observations (Note, not all of these are required and used) torso_pos, torso_quat, torso_vel, torso_angular_vel, joint_angles, joint_velocities, joint_torques, contacts = self.get_obs( ) xd, yd, zd = torso_vel # Unpack for clarity roll, pitch, yaw = p.getEulerFromQuaternion(torso_quat) self.step_ctr += 1 """reward """ # try to make him turn legs from the car l = p.getLinkStates(self.robot, [5, 9]) l2 = p.getLinkStates(self.robot, [12, 16]) torso_z = (p.getLinkStates(self.robot, [0]))[0][4][2] vec1 = (l[1][0][0] - l[0][0][0], l[1][0][1] - l[0][0][1], l[1][0][2] - l[0][0][2]) vec2 = (l2[1][0][0] - l2[0][0][0], l2[1][0][1] - l2[0][0][1], l2[1][0][2] - l2[0][0][2]) vec_vychozi = [0, 1, 0] skalarni_soucin1 = vec1[0] * vec_vychozi[0] + vec1[1] * vec_vychozi[ 1] + vec1[2] * vec_vychozi[2] angle1 = math.acos(skalarni_soucin1 / math.sqrt(vec1[0] * vec1[0] + vec1[1] * vec1[1] + vec1[2] * vec1[2])) skalarni_soucin2 = vec2[0] * vec_vychozi[0] + vec2[1] * vec_vychozi[ 1] + vec2[2] * vec_vychozi[2] angle2 = math.acos(skalarni_soucin2 / math.sqrt(vec2[0] * vec2[0] + vec2[1] * vec2[1] + vec2[2] * vec2[2])) # 2<0;pi> -> good<0;2>bad legs_rot = ((angle1) + (angle2)) / (pi) # <-pi;pi> -> <-3pi/2;pi/2> -> <0;3pi/2> -> good<0;1>bad torso_good_rot = abs(yaw - pi / 2) * 2 / (3 * pi) # 2<-pi;pi> -> 2<0;pi> -> good<0;1>bad joint_angles_n = ((joint_angles - self.joints_rad_low) / self.joints_rad_diff) * 2 - 1 yaw = ((yaw + pi) / (2 * pi)) * 2 - 1 roll = ((roll + pi) / (2 * pi)) * 2 - 1 pitch = ((pitch + pi) / (2 * pi)) * 2 - 1 feet, pelvis, neck = self.feet_pelvis_neck_movement() r_neg = neck * 2 + pelvis * 3 + feet r_pos = (1 - torso_good_rot) * 2 + ( 2 - legs_rot) + (1 - (abs(roll) + abs(pitch)) / 2) * 3 r = np.clip(r_pos - r_neg, -5, 5) """reward """ # get joint_angles into np array and make observation env_obs = np.concatenate( (torso_pos, [roll, pitch, yaw], joint_angles_n, joint_velocities, contacts)).astype(np.float32) # This condition terminates the episode - WARNING - it can cause that the robot will try # terminate the episode as quickly as possible #done = self.step_ctr > self.max_steps or (np.abs(roll) > 1.5 or np.abs(pitch) > 1.5) or torso_z < 1 or (legs_rot <.2 and abs(yaw-pi/2) < .2) done = self.step_ctr > self.max_steps or ( np.abs(roll) > .5 or np.abs(pitch) > .5) or torso_z < 1 or ( angle1 < .05 and angle2 < .05 and abs(yaw - .5) < .1) # TODO: why random element? env_obs_noisy = env_obs # + np.random.rand(self.obs_dim).astype(np.float32) * 0.1 - 0.05 info = { 'rotated': (angle1 < .2 and angle2 < .2 and abs(yaw - .5) < .2) } #,"joints":joint_angles,"pos":torso_pos,"quat":torso_quat} #info = {} return env_obs_noisy, r, done, info
def get_xyz_from_index(self, index): return list([ p.getLinkStates(self.urdf, [index])[0][0][0], p.getLinkStates(self.urdf, [index])[0][0][1], p.getLinkStates(self.urdf, [index])[0][0][2] ])
def robot_coords(robotID): states = p.getLinkStates(robotID, range(11)) coords = np.array([s[0] for s in states]) return coords[1:8]
##currentPosition=state[0] , #force=1) #move link to target position #uncomment this #lockJoint #p.createConstraint(parentBodyUniqueId=plane, parentLinkIndex=-1, childBodyUniqueId=plane, childLinkIndex=0, jointType=p.JOINT_FIXED, jointAxis=[0,0,0], parentFramePosition=[2,2,2], childFramePosition=[2,2,2] ) #########LOCK joint with force #p.setJointMotorControl2(bodyIndex=plane, jointIndex=0, controlMode=p.VELOCITY_CONTROL, targetVelocity=0, force=10000)#lock joint with large force #p.setJointMotorControl2(bodyIndex=plane, jointIndex=0, controlMode=p.VELOCITY_CONTROL, targetVelocity=1, force=0) #free all joint constraints with force 0 while(1): # print(angles) ( ( l1, l2, l3, l4, l5, l6 ), ) = p.getLinkStates(bodyUniqueId=plane, linkIndices=[3] ) #returns tuple end_effector_world_position_cartesian = list(l1) #l = p.getLinkStates(bodyUniqueId=plane, linkIndices=[3] ) #pint out EE cartesian coordinates print("The end effector world position in Cartesian coordinates is: [x,y,z] ") print(type(end_effector_world_position_cartesian)) print( end_effector_world_position_cartesian ) #print out EE polar coordinates print("The end effector posiaiton in Polar coordinates are: [radius, theta, phi]") #print(end_effector_world_position_cartesian) end_effector_world_position_polar = coordinates.utils.cartesian_to_spherical_novel(end_effector_world_position_cartesian[0], end_effector_world_position_cartesian[1], end_effector_world_position_cartesian[2] ) print( type(end_effector_world_position_polar) ) print(end_effector_world_position_polar)
def get_link_states(self, joint_indices, **kwargs): """ Get the states of all movable links and return LinkState, which is a structure of arrays (SoA). """ return p.getLinkStates(self.id, joint_indices, **self._client_kwargs, **kwargs)
p.setJointMotorControl2(RoboBoi, 8, p.POSITION_CONTROL, \ targetPosition = (0.1 + 0.1*math.sin(0.1*i))) else: target1 = 0 target2 = 0 target3 = 0 target4 = 0 all_target_1[i] = target1 all_target_2[i] = target2 all_target_3[i] = target3 all_target_4[i] = target4 target1_speed, target2_speed, target3_speed, target4_speed = \ p.getLinkStates(RoboBoi, [0, 3, 9, 6], computeLinkVelocity=1) link0_velocity_vec = target1_speed[6] link0_velocity_mag = numpy.linalg.norm(link0_velocity_vec) link3_velocity_vec = target2_speed[6] link3_velocity_mag = numpy.linalg.norm(link3_velocity_vec) link9_velocity_vec = target3_speed[6] link9_velocity_mag = numpy.linalg.norm(link9_velocity_vec) link6_velocity_vec = target4_speed[6] link6_velocity_mag = numpy.linalg.norm(link6_velocity_vec) all_target_1[i, 1] = link0_velocity_mag all_target_2[i, 1] = link3_velocity_mag all_target_3[i, 1] = link9_velocity_mag
def write(): # will write "ROBOTICS" arr = [[-6, 0], [-6, 2], [-5, 2], [-5, 1], [-6, 1], [-5, 0], [-3, 0], [-3, 2], [-4, 2], [-4, 0], [-1, 0], [-1, 2], [-2, 2], [-2, 1], [-1, 1], [-2, 1], [-2, 0], [1, 0], [1, 2], [0, 2], [0, 0], [3, 0], [3, 2], [2, 2], [4, 2], [3, 2], [3, 0], [5, 0], [5, 2], [5, 0], [6, 0], [6, 2], [7, 2], [6, 2], [6, 0], [8, 0], [9, 0], [9, 1], [8, 1], [8, 2], [9.5, 2]] X = [] Y = [] Z = [] kukaBasePosition = p.getLinkStates(kukaId, [0])[0][0] kukaBaseOrientation = p.getEulerFromQuaternion( p.getLinkStates(husky, [0])[0][1]) py, pz = arr[0] py = py / 10 pz = 1 + pz / 10 px = 0.7 q1, q2, q3, q4, q5, q6 = get_kuka_angles(kukaBasePosition, kukaBaseOrientation, [px, py, pz], [0, 0, 0]) p.setJointMotorControlArray(kukaId, range(11), p.POSITION_CONTROL, targetPositions=[ q1, q2, q3, q4, q5, q6, curr_joint_value[6], curr_joint_value[7], curr_joint_value[8], curr_joint_value[9], curr_joint_value[10] ]) time.sleep(1) prevPose1 = [px, py, pz] hasPrevPose = 0 trailDuration = 30 t = 0 # parametric variable fig, ax = create_plot() for i in range(len(arr)): py, pz = arr[i] py = py / 10 pz = 1 + pz / 10 px = 0.7 # print(px, py, pz) X.append(px) Y.append(py) Z.append(pz) # getting values of each joints according to points q1, q2, q3, q4, q5, q6 = get_kuka_angles(kukaBasePosition, kukaBaseOrientation, [px, py, pz], [0, 0, 0]) p.setJointMotorControlArray( kukaId, range(11), p.POSITION_CONTROL, targetPositions=[ q1, q2, q3, q4, q5, q6, curr_joint_value[6], curr_joint_value[7], curr_joint_value[8], curr_joint_value[9], curr_joint_value[10] ]) t += 1 ls = p.getLinkState(kukaId, 5) if (hasPrevPose): p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 4, trailDuration) # prevPose = pos prevPose1 = ls[4] hasPrevPose = 1 time.sleep(.2) update_plot(X, Y, Z, fig, ax, 41) plt.show()
def __init__(self, q_init, envID, use_flat_plane, enable_pyb_GUI, dt=0.001): self.applied_force = np.zeros(3) # Start the client for PyBullet if enable_pyb_GUI: pyb.connect(pyb.GUI) else: pyb.connect(pyb.DIRECT) # p.GUI for graphical version # p.DIRECT for non-graphical version # Load horizontal plane pyb.setAdditionalSearchPath(pybullet_data.getDataPath()) # Either use a flat ground or a rough terrain if use_flat_plane: self.planeId = pyb.loadURDF("plane.urdf") # Flat plane self.planeIdbis = pyb.loadURDF("plane.urdf") # Flat plane pyb.resetBasePositionAndOrientation(self.planeIdbis, [20.0, 0, 0], [0, 0, 0, 1]) else: import random random.seed(41) # p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) heightPerturbationRange = 0.05 numHeightfieldRows = 256*2 numHeightfieldColumns = 256*2 heightfieldData = [0]*numHeightfieldRows*numHeightfieldColumns height_prev = 0.0 for j in range(int(numHeightfieldColumns/2)): for i in range(int(numHeightfieldRows/2)): # uniform distribution height = random.uniform(0, heightPerturbationRange) # height = 0.25*np.sin(2*np.pi*(i-128)/46) # sinus pattern heightfieldData[2*i+2*j * numHeightfieldRows] = (height + height_prev) * 0.5 heightfieldData[2*i+1+2*j*numHeightfieldRows] = height heightfieldData[2*i+(2*j+1) * numHeightfieldRows] = (height + height_prev) * 0.5 heightfieldData[2*i+1+(2*j+1)*numHeightfieldRows] = height height_prev = height # Create the collision shape based on the height field terrainShape = pyb.createCollisionShape(shapeType=pyb.GEOM_HEIGHTFIELD, meshScale=[.05, .05, 1], heightfieldTextureScaling=(numHeightfieldRows-1)/2, heightfieldData=heightfieldData, numHeightfieldRows=numHeightfieldRows, numHeightfieldColumns=numHeightfieldColumns) self.planeId = pyb.createMultiBody(0, terrainShape) pyb.resetBasePositionAndOrientation(self.planeId, [0, 0, 0], [0, 0, 0, 1]) pyb.changeVisualShape(self.planeId, -1, rgbaColor=[1, 1, 1, 1]) if envID == 1: # Add stairs with platform and bridge self.stairsId = pyb.loadURDF("bauzil_stairs.urdf") # , """basePosition=[-1.25, 3.5, -0.1], baseOrientation=pyb.getQuaternionFromEuler([0.0, 0.0, 3.1415]))""" pyb.changeDynamics(self.stairsId, -1, lateralFriction=1.0) # Create the red steps to act as small perturbations mesh_scale = [1.0, 0.1, 0.02] visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH, fileName="cube.obj", halfExtents=[0.5, 0.5, 0.1], rgbaColor=[1.0, 0.0, 0.0, 1.0], specularColor=[0.4, .4, 0], visualFramePosition=[0.0, 0.0, 0.0], meshScale=mesh_scale) collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH, fileName="cube.obj", collisionFramePosition=[0.0, 0.0, 0.0], meshScale=mesh_scale) for i in range(4): tmpId = pyb.createMultiBody(baseMass=0.0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[0.0, 0.5+0.2*i, 0.01], useMaximalCoordinates=True) pyb.changeDynamics(tmpId, -1, lateralFriction=1.0) tmpId = pyb.createMultiBody(baseMass=0.0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[0.5, 0.5+0.2*4, 0.01], useMaximalCoordinates=True) pyb.changeDynamics(tmpId, -1, lateralFriction=1.0) tmpId = pyb.createMultiBody(baseMass=0.0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[0.5, 0.5+0.2*5, 0.01], useMaximalCoordinates=True) pyb.changeDynamics(tmpId, -1, lateralFriction=1.0) # Create the green steps to act as bigger perturbations mesh_scale = [0.2, 0.1, 0.01] visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH, fileName="cube.obj", halfExtents=[0.5, 0.5, 0.1], rgbaColor=[0.0, 1.0, 0.0, 1.0], specularColor=[0.4, .4, 0], visualFramePosition=[0.0, 0.0, 0.0], meshScale=mesh_scale) collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH, fileName="cube.obj", collisionFramePosition=[0.0, 0.0, 0.0], meshScale=mesh_scale) for i in range(3): tmpId = pyb.createMultiBody(baseMass=0.0, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[0.15 * (-1)**i, 0.9+0.2*i, 0.025], useMaximalCoordinates=True) pyb.changeDynamics(tmpId, -1, lateralFriction=1.0) # Create sphere obstacles that will be thrown toward the quadruped mesh_scale = [0.1, 0.1, 0.1] visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH, fileName="sphere_smooth.obj", halfExtents=[0.5, 0.5, 0.1], rgbaColor=[1.0, 0.0, 0.0, 1.0], specularColor=[0.4, .4, 0], visualFramePosition=[0.0, 0.0, 0.0], meshScale=mesh_scale) collisionShapeId = pyb.createCollisionShape(shapeType=pyb.GEOM_MESH, fileName="sphere_smooth.obj", collisionFramePosition=[0.0, 0.0, 0.0], meshScale=mesh_scale) self.sphereId1 = pyb.createMultiBody(baseMass=0.4, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[-0.6, 0.9, 0.1], useMaximalCoordinates=True) self.sphereId2 = pyb.createMultiBody(baseMass=0.4, baseInertialFramePosition=[0, 0, 0], baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex=visualShapeId, basePosition=[0.6, 1.1, 0.1], useMaximalCoordinates=True) # Flag to launch the two spheres in the environment toward the robot self.flag_sphere1 = True self.flag_sphere2 = True # Create blue spheres without collision box for debug purpose mesh_scale = [0.015, 0.015, 0.015] visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH, fileName="sphere_smooth.obj", halfExtents=[0.5, 0.5, 0.1], rgbaColor=[0.0, 0.0, 1.0, 1.0], specularColor=[0.4, .4, 0], visualFramePosition=[0.0, 0.0, 0.0], meshScale=mesh_scale) self.ftps_Ids = np.zeros((4, 5), dtype=np.int) for i in range(4): for j in range(5): self.ftps_Ids[i, j] = pyb.createMultiBody(baseMass=0.0, baseInertialFramePosition=[0, 0, 0], baseVisualShapeIndex=visualShapeId, basePosition=[0.0, 0.0, -0.1], useMaximalCoordinates=True) # Create green spheres without collision box for debug purpose visualShapeId = pyb.createVisualShape(shapeType=pyb.GEOM_MESH, fileName="sphere_smooth.obj", halfExtents=[0.5, 0.5, 0.1], rgbaColor=[0.0, 1.0, 0.0, 1.0], specularColor=[0.4, .4, 0], visualFramePosition=[0.0, 0.0, 0.0], meshScale=mesh_scale) self.ftps_Ids_deb = [0] * 4 for i in range(4): self.ftps_Ids_deb[i] = pyb.createMultiBody(baseMass=0.0, baseInertialFramePosition=[0, 0, 0], baseVisualShapeIndex=visualShapeId, basePosition=[0.0, 0.0, -0.1], useMaximalCoordinates=True) """cubeStartPos = [0.0, 0.45, 0.0] cubeStartOrientation = pyb.getQuaternionFromEuler([0, 0, 0]) self.cubeId = pyb.loadURDF("cube_small.urdf", cubeStartPos, cubeStartOrientation) pyb.changeDynamics(self.cubeId, -1, mass=0.5) print("Mass of cube:", pyb.getDynamicsInfo(self.cubeId, -1)[0])""" # Set the gravity pyb.setGravity(0, 0, -9.81) # Load Quadruped robot robotStartPos = [0, 0, 0.0] robotStartOrientation = pyb.getQuaternionFromEuler([0.0, 0.0, 0.0]) # -np.pi/2 pyb.setAdditionalSearchPath("/opt/openrobots/share/example-robot-data/robots/solo_description/robots") self.robotId = pyb.loadURDF("solo12.urdf", robotStartPos, robotStartOrientation) # Disable default motor control for revolute joints self.revoluteJointIndices = [0, 1, 2, 4, 5, 6, 8, 9, 10, 12, 13, 14] pyb.setJointMotorControlArray(self.robotId, jointIndices=self.revoluteJointIndices, controlMode=pyb.VELOCITY_CONTROL, targetVelocities=[0.0 for m in self.revoluteJointIndices], forces=[0.0 for m in self.revoluteJointIndices]) # Initialize the robot in a specific configuration self.q_init = np.array([q_init]).transpose() pyb.resetJointStatesMultiDof(self.robotId, self.revoluteJointIndices, self.q_init) # q0[7:]) # Enable torque control for revolute joints jointTorques = [0.0 for m in self.revoluteJointIndices] pyb.setJointMotorControlArray(self.robotId, self.revoluteJointIndices, controlMode=pyb.TORQUE_CONTROL, forces=jointTorques) # Get position of feet in world frame with base at (0, 0, 0) feetLinksID = [3, 7, 11, 15] linkStates = pyb.getLinkStates(self.robotId, feetLinksID) # Get minimum height of feet (they are in the ground since base is at 0, 0, 0) z_min = linkStates[0][4][2] i_min = 0 i = 1 for link in linkStates[1:]: if link[4][2] < z_min: z_min = link[4][2] i_min = i i += 1 # Set base at (0, 0, -z_min) so that the lowest foot is at z = 0 pyb.resetBasePositionAndOrientation(self.robotId, [0.0, 0.0, -z_min], [0, 0, 0, 1]) # Progressively raise the base to achieve proper contact (take into account radius of the foot) while (pyb.getClosestPoints(self.robotId, self.planeId, distance=0.005, linkIndexA=feetLinksID[i_min]))[0][8] < 0.001: z_min -= 0.001 pyb.resetBasePositionAndOrientation(self.robotId, [0.0, 0.0, -z_min], [0, 0, 0, 1]) # Fix the base in the world frame # pyb.createConstraint(self.robotId, -1, -1, -1, pyb.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, robotStartPos[2]]) # Set time step for the simulation pyb.setTimeStep(dt) # Change camera position pyb.resetDebugVisualizerCamera(cameraDistance=0.6, cameraYaw=45, cameraPitch=-39.9, cameraTargetPosition=[0.0, 0.0, robotStartPos[2]-0.2])
def _get_observation(self): # Obtain relevant quantities: Links (handle_inside, handle_outside, door) = pybullet.getLinkStates( self.body_name_2_joints['door']['body_id'], linkIndices=[ self.door_link_IDs['handle_inside'], self.door_link_IDs['handle_outside'], self.door_link_IDs['door_COM'] ], computeLinkVelocity=1) (torso, right_hand, left_hand) = pybullet.getLinkStates( self.body_name_2_joints['robot']['body_id'], linkIndices=[ self.robot_link_IDs['torso'], self.robot_link_IDs['right_hand'], self.robot_link_IDs['left_hand'] ], computeLinkVelocity=1) # Obtain relevant quantities: Joints (door_hinge, handle_hinge) = pybullet.getJointStates( self.body_name_2_joints['door']['body_id'], self.door_joints_index) robot_joints = pybullet.getJointStates( self.body_name_2_joints['robot']['body_id'], self.robot_joints_index) # Put them in a dictionary obs_dict = { 'handle_inside_pos': np.array(handle_inside[0]), # 'handle_inside_vel': np.array(handle_inside[6]), 'handle_outside_pos': np.array(handle_outside[0]), # 'handle_outside_vel': np.array(handle_outside[6]), 'door_pos': np.array(door[0]), # 'door_vel': np.array(door[6]), 'torso_pos': np.array(torso[0]), # 'torso_vel': np.array(torso[6]), 'right_hand_pos': np.array(right_hand[0]), # 'right_hand_vel': np.array(right_hand[6]), 'left_hand_pos': np.array(left_hand[0]), # 'left_hand_vel': np.array(left_hand[6]), 'door_hinge_pos': np.array([door_hinge[0]]), # 'door_hinge_vel': np.array([door_hinge[1]]), 'handle_hinge_pos': np.array([handle_hinge[0]]), # 'handle_hinge_vel': np.array([handle_hinge[1]]), 'robot_joint_pos': np.array([robot_joints[i][0] for i in range(len(robot_joints))]), # 'robot_joint_vel': np.array([robot_joints[i][1] for i in range(len(robot_joints))]) } # Flatten into raw observation list obs_list = np.array( [entry for obs in obs_dict.values() for entry in obs]) return obs_list, obs_dict
p.getJointInfo(bodyUniqueId=octopusBodyUniqueId, jointIndex=0, physicsClientId=physicsClientId) #query all joint info p.getJointInfo(bodyUniqueId=octopusBodyUniqueId,jointIndex=0)[12] # joint's child link name (helps with debugging) #p.getJointInfos() doesnt exist ! #query the joint state(s) p.getJointState(bodyUniqueId=octopusBodyUniqueId, jointIndex=0, physicsClientId=physicsClientId) p.getJointStates(bodyUniqueId=octopusBodyUniqueId, jointIndices=list(range(number_of_links_urdf)), physicsClientId=physicsClientId) #query the link info #p.getLinkInfo() doesnt exist ! #p.getLinkInfos() doesnt exist ! #query the link state (linkWorldPosition, linkWorldOrientation, localInertialFramePosition, localInertialFrameOrientation, worldLinkFramePosition, worldLinkFrameOrientation, worldLinkLinearVelocity, worldLinkAngularVelocity) = p.getLinkState(bodyUniqueId=octopusBodyUniqueId, linkIndex=0, computeLinkVelocity=1, computeForwardKinematics=1, physicsClientId=physicsClientId) #list items 0,1 are same as list items 4,5 for octopus 8-link case #CoM frame position = inertial Frame position and orientation # for 8-link octopusarms list items [2,3] are localInertialFramePosition=[0,0,0] and localInertialFrameOrientation=[0,0,0,1] = p.getLinkStates( ) #list items 0,1 are same as list items 4,5 for octopus 8-link case #CoM frame position = inertial Frame position and orientation, localInertianFramePosition=[0,0,0], localInertialFrameOrientation=[0,0,0,0] #LOCK A JOINT #get joint's child link name (for sanity check to ensure that we are indexing correct values from the list) jointChildLinkName=jointParentFramePosition=p.getJointInfo(self.octopusBodyUniqueId, jointIndex=0, physicsClientId=self.physicsClientId)[12] #get joint frame position, relative to parent link frame jointFramePosition_WRT_ParentLinkFrame=p.getJointInfo(octopusBodyUniqueId, jointIndex=0, physicsClientId=physicsClientId)[14] #get joint frame orientation, relative to parent CoM link frame jointFrameOrientation_WRT_ParentLinkFrame=p.getJointInfo(octopusBodyUniqueId, jointIndex=0, physicsClientId=physicsClientId)[15] #get position of the joint frame, relative to a given child CoM Coordidinate Frame, (other posibility: get world origin (0,0,0) if no child specified for this joint) jointsChildLinkFramePosition_WRT_WorldFramePosition = p.getJointState() jointsFramePosition_WRT_WorldFramePosition =