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 =
