def setStartingPositionAndVelocity(self, inputFrame):
# set the agent's root position and orientation
p.resetBasePositionAndOrientation(
self.humanoidAgent,
posObj=inputFrame[0:3],
ornObj=inputFrame[3:7]
)
# set the agent's root velocity and angular velocity
p.resetBaseVelocity(
self.humanoidAgent,
linearVelocity=inputFrame[7:10],
angularVelocity=inputFrame[10:13]
)
# Set joint positions
revoluteJointIndices = [13, 15, 17, 19]
for i, joint in enumerate(self.revoluteJoints):
p.resetJointState(
self.humanoidAgent,
jointIndex=joint,
targetValue=inputFrame[revoluteJointIndices[i]],
targetVelocity=inputFrame[revoluteJointIndices[i]+1]
)
sphericalJointIndices = [21, 28, 35, 42, 49, 56, 63, 70]
p.resetJointStatesMultiDof(
self.humanoidAgent,
jointIndices=self.sphericalJoints,
targetValues=[inputFrame[index:index+4] for index in sphericalJointIndices],
targetVelocities=[inputFrame[index+4:index+7] for index in sphericalJointIndices]
)
# slight delay is needed before agent will reset. Need to investigate before removing this delay
time.sleep(0.005)
def setStartingPositionAndVelocity(self, inputFrame):
sphericalJointIndices = [0]
p.resetJointStatesMultiDof(
self.humanoidAgent,
jointIndices=self.sphericalJoints,
targetValues=[inputFrame[index:index+4] for index in sphericalJointIndices],
targetVelocities=[inputFrame[index+4:index+7] for index in sphericalJointIndices]
)
# slight delay is needed before agent will reset. Need to investigate before removing this delay
time.sleep(0.005)
def main():
val = vs()
# Set val initial pose
# high cond number
# init_pos = [[0.2]] * len(val.left_arm_joint_indices)
init_pos2 = []
for joint in val.left_arm_joints:
init_pos2.append([val.home[joint]])
print(init_pos2)
init_pos = [[0.3]] * len(val.left_arm_joint_indices)
p.resetJointStatesMultiDof(val.robot,
jointIndices=val.left_arm_joint_indices,
targetValues=init_pos2)
time.sleep(0.5)
# Optional cartesian velocity controller test
val.cart_vel_linear_test()
val.cart_vel_angular_test()
# Get Jacobian testing script
jac_trn, jac_rot = get_jacobian(val.robot, val.left_tool)
jac = get_arm_jacobian(val.robot, "left", val.left_tool)
# Get pose testing script
pos, rot = get_pose(val.robot, val.left_tool)
print("axis angle result: {}".format(rot))
# Define and draw goal pose
cur_pos, cur_rot = get_pose(val.robot, val.left_tool)
goal_pos = tuple(
np.asarray(np.array(cur_pos) + np.array([0.05, 0.05, -0.1])))
goal_rot = p.getQuaternionSlerp(cur_rot, (0, 0, 0, 1), 0.4)
draw_pose(cur_pos, cur_rot)
draw_pose(goal_pos, goal_rot)
# camera test
camera_test()
val.pbvs("left",
goal_pos,
goal_rot,
kv=1.0,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
plot_pose=True,
perturb_jacobian=False,
perturb_Jac_joint_mu=0.2,
perturb_Jac_joint_sigma=0.2,
perturb_orientation=False,
mu_R=0.3,
sigma_R=0.3,
plot_result=True)
p.disconnect()
def __init__(self, dt=0.001):
# Start the client for PyBullet
physicsClient = pyb.connect(pyb.GUI)
# p.GUI for graphical version
# p.DIRECT for non-graphical version
# Load horizontal plane
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
self.planeId = pyb.loadURDF("plane.urdf")
# Set the gravity
pyb.setGravity(0, 0, -9.81)
# Load Quadruped robot
robotStartPos = [0, 0, 0.235 + 0.0045]
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
straight_standing = np.array(
[[0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8,
1.6]]).transpose()
pyb.resetJointStatesMultiDof(self.robotId, self.revoluteJointIndices,
straight_standing) # 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)
# Set time step for the simulation
pyb.setTimeStep(dt)
# Change camera position
pyb.resetDebugVisualizerCamera(cameraDistance=0.6,
cameraYaw=-50,
cameraPitch=-35,
cameraTargetPosition=[0.0, 0.6, 0.0])
def reset(self, pose=[[0, 0, 0.3], [0, 0, 0, 1]]):
p.resetBasePositionAndOrientation(self.cliffordID,
pose[0],
pose[1],
physicsClientId=self.physicsClientId)
nJoints = p.getNumJoints(self.cliffordID,
physicsClientId=self.physicsClientId)
p.resetJointStatesMultiDof(self.cliffordID,
range(nJoints),
self.initialJointPositions,
self.initialJointVelocities,
physicsClientId=self.physicsClientId)
def main():
val = vs()
# Set val initial pose
# high cond number
# init_pos = [[0.2]] * len(val.left_arm_joint_indices)
init_pos2 = []
for joint in val.left_arm_joints:
init_pos2.append([val.home[joint]])
init_pos = [[0.3]] * len(val.left_arm_joint_indices)
p.resetJointStatesMultiDof(val.robot,
jointIndices=val.left_arm_joint_indices,
targetValues=init_pos2)
time.sleep(0.5)
val.pbvs_pf_test(print_init_poses=False)
time.sleep(10)
p.disconnect()
def render_img(self,
left_gripper_joint,
left_gripper2_joint,
Tce,
plot_cam_pose=False,
imsize=(214, 214),
plot_img=False,
show_time_taken=False):
if show_time_taken:
start = time.time()
p.resetJointStatesMultiDof(self.robot,
jointIndices=self.joint_ids,
targetValues=[[left_gripper_joint],
[left_gripper2_joint]],
physicsClientId=self.physicsClient)
Tec = Tce.inverse()
cam_trans, cam_rot = SE32_trans_rot(Tec)
cam_rotm = Tec.rotationMatrix()
if plot_cam_pose:
draw_pose(cam_trans, cam_rot)
# Calculate extrinsic matrix
# target position is camera frame y axis tip in global frame
# up vector is the z axis of camera frame
viewMatrix = p.computeViewMatrix(
cameraEyePosition=cam_trans,
cameraTargetPosition=(cam_rotm.dot(np.array([0, 1, 0]).T) +
np.array(cam_trans)).tolist(),
cameraUpVector=cam_rotm[:, 2].tolist())
width, height, rgbImg, depthImg, segImg = p.getCameraImage(
width=imsize[0],
height=imsize[1],
viewMatrix=viewMatrix,
projectionMatrix=self.projectionMatrix,
flags=p.ER_NO_SEGMENTATION_MASK,
physicsClientId=self.physicsClient)
if show_time_taken:
print("time taken for rendering: {}".format(time.time() - start))
if plot_img:
plt.imshow(rgbImg[:, :, :3])
plt.show()
return rgbImg[:, :, :3]
def main():
val = vs()
# Set val initial pose
init_pos = []
for joint in val.left_arm_joints:
init_pos.append([val.home[joint]])
p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos)
time.sleep(0.5)
# joint velocity controller test - linear velocity
print("joint velocity controller test - linear velocity")
val.cart_vel_linear_test(t=1.5, v=0.05)
# joint velocity controller test - angular velocity
print("joint velocity controller test - angular velocity")
val.cart_vel_angular_test(t=1.5, w=0.25)
# Define and draw goal pose
cur_pos, cur_rot = get_pose(val.robot, val.left_tool)
goal_pos_global = tuple(np.asarray(np.array(cur_pos) + np.array([0.05, 0.05, -0.1])))
goal_rot_global = p.getQuaternionSlerp(cur_rot, (0, 0, 0, 1), 0.4) # weird thing happens if it is 0.7
draw_pose(cur_pos, cur_rot)
draw_pose(goal_pos_global, goal_rot_global)
# transform goal_pose to camera frame
goal_pos, goal_rot = SE32_trans_rot(val.Trc.inverse() * trans_rot2SE3(goal_pos_global, goal_rot_global))
# Position Based Visual Servoing with no perturbation
print("Position Based Visual Servoing with no perturbation")
val.pbvs("left", goal_pos, goal_rot,
kv=1.0,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
plot_pose=True,
perturb_jacobian=False,
perturb_orientation=False,
plot_result=True)
# reset to home pose
p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos)
draw_pose(cur_pos, cur_rot)
draw_pose(goal_pos_global, goal_rot_global)
# Position Based Visual Servoing with perturbation in Jacobian
print("Position Based Visual Servoing with perturbation in Jacobian")
val.pbvs("left", goal_pos, goal_rot,
kv=1.0,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
plot_pose=True,
perturb_jacobian=True,
perturb_Jac_joint_mu=0.2,
perturb_Jac_joint_sigma=0.2,
perturb_orientation=False,
mu_R=0.3,
sigma_R=0.3,
plot_result=True)
# reset to home pose
p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos)
draw_pose(cur_pos, cur_rot)
draw_pose(goal_pos_global, goal_rot_global)
# execute Particle-Filter based PBVS, using default PBVS parameters which can be changed
val.pbvs_pf("left", goal_pos, goal_rot)
p.disconnect()
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 __init__(self, dt=0.001):
# Start the client for PyBullet
physicsClient = pyb.connect(pyb.DIRECT)
# p.GUI for graphical version
# p.DIRECT for non-graphical version
# Load horizontal plane
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
self.planeId = pyb.loadURDF("plane.urdf")
#self.stairsId = pyb.loadURDF("../../../../../Documents/Git-Repositories/mpc-tsid/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)
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)
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)
mesh_scale = [0.05, 0.05, 0.05]
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.3,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[-0.6, 0.9, 0.05],
useMaximalCoordinates=True)
self.sphereId2 = pyb.createMultiBody(
baseMass=0.3,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0.6, 1.1, 0.05],
useMaximalCoordinates=True)
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)
# Flag to launch the two spheres in the environment toward the robot
self.flag_sphere1 = True
self.flag_sphere2 = True
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)
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.235 + 0.0045]
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
straight_standing = np.array(
[[0, 0.8, -1.6, 0, 0.8, -1.6, 0, -0.8, 1.6, 0, -0.8,
1.6]]).transpose()
pyb.resetJointStatesMultiDof(self.robotId, self.revoluteJointIndices,
straight_standing) # 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)
# Fix the base in the world frame
# p.createConstraint(robotId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 0.34])
# Set time step for the simulation
pyb.setTimeStep(dt)
# Change camera position
pyb.resetDebugVisualizerCamera(cameraDistance=0.6,
cameraYaw=-50,
cameraPitch=-35,
cameraTargetPosition=[0.0, 0.6, 0.0])
def __init__(self,
urdf_name,
dt,
q_init,
nqa,
pinocchio_robot,
joint_names,
contact_frame_names,
guion=True,
gravity=[0, 0, -9.81]):
"""
contact_frame_names: ! to work, contact_frame_names needs to be included in the joint frame names
"""
# Start the client for PyBullet
if guion:
pyb.connect(pyb.GUI)
else:
pyb.connect(pyb.DIRECT)
# Load horizontal plane
pyb.setAdditionalSearchPath(pybullet_data.getDataPath())
self.planeId = pyb.loadURDF("plane.urdf")
# Set the gravity
pyb.setGravity(*gravity)
# Load Quadruped robot with init config (with containing freeflyer pose)
robotStartPos = q_init[:3]
robotStartOrientation = q_init[3:7]
q_a = q_init[7:].reshape((nqa, 1))
pyb.setAdditionalSearchPath(
"/opt/openrobots/share/example-robot-data/robots/solo_description/robots"
)
flags = pyb.URDF_USE_INERTIA_FROM_FILE # Necessary?
self.robotId = pyb.loadURDF(urdf_name,
robotStartPos,
robotStartOrientation,
flags=flags)
# get joint ids informations in pyb and pinocchio
bullet_joint_map = {}
for ji in range(pyb.getNumJoints(self.robotId)):
bullet_joint_map[pyb.getJointInfo(self.robotId,
ji)[1].decode('UTF-8')] = ji
self.bullet_joint_ids = np.array(
[bullet_joint_map[name] for name in joint_names])
self.pinocchio_joint_ids = np.array(
[pinocchio_robot.model.getJointId(name) for name in joint_names])
# In pybullet, the contact wrench is measured at a joint. In our case
# the joint is fixed joint. Pinocchio doesn't add fixed joints into the joint
# list. Therefore, the computation is done wrt to the frame of the fixed joint.
self.pyb_ct_frame_ids = [
bullet_joint_map[name] for name in contact_frame_names
]
self.pin_ct_frame_ids = [
pinocchio_robot.model.getFrameId(name)
for name in contact_frame_names
]
# Disable default motor control for revolute joints
pyb.setJointMotorControlArray(
self.robotId,
jointIndices=self.bullet_joint_ids,
controlMode=pyb.VELOCITY_CONTROL,
forces=[0.0 for _ in self.bullet_joint_ids])
# Initialize the robot in a specific configuration
pyb.resetJointStatesMultiDof(self.robotId, self.bullet_joint_ids, q_a)
# Enable torque control for revolute joints
jointTorques = [0.0] * len(self.bullet_joint_ids)
pyb.setJointMotorControlArray(self.robotId,
self.bullet_joint_ids,
controlMode=pyb.TORQUE_CONTROL,
forces=jointTorques)
# Set time step for the simulation
pyb.setTimeStep(dt)
# Change camera position
pyb.resetDebugVisualizerCamera(cameraDistance=0.6,
cameraYaw=-50,
cameraPitch=-35,
cameraTargetPosition=[0.0, 0.6, 0.0])