def joinUrdf(self, childEditor, parentLinkIndex=0, jointPivotXYZInParent=[0,0,0], jointPivotRPYInParent=[0,0,0], jointPivotXYZInChild=[0,0,0], jointPivotRPYInChild=[0,0,0], parentPhysicsClientId=0, childPhysicsClientId=0): childLinkIndex = len(self.urdfLinks) insertJointIndex = len(self.urdfJoints) #combine all links, and add a joint for link in childEditor.urdfLinks: self.linkNameToIndex[link.link_name]=len(self.urdfLinks) self.urdfLinks.append(link) for joint in childEditor.urdfJoints: self.urdfJoints.append(joint) #add a new joint between a particular jointPivotQuatInChild = p.getQuaternionFromEuler(jointPivotRPYInChild) invJointPivotXYZInChild, invJointPivotQuatInChild = p.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild) #apply this invJointPivot***InChild to all inertial, visual and collision element in the child link #inertial pos, orn = p.multiplyTransforms(self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,p.getQuaternionFromEuler(self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild, physicsClientId=parentPhysicsClientId) self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p.getEulerFromQuaternion(orn) #all visual for v in self.urdfLinks[childLinkIndex].urdf_visual_shapes: pos, orn = p.multiplyTransforms(v.origin_xyz,p.getQuaternionFromEuler(v.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild) v.origin_xyz = pos v.origin_rpy = p.getEulerFromQuaternion(orn) #all collision for c in self.urdfLinks[childLinkIndex].urdf_collision_shapes: pos, orn = p.multiplyTransforms(c.origin_xyz,p.getQuaternionFromEuler(c.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild) c.origin_xyz = pos c.origin_rpy = p.getEulerFromQuaternion(orn) childLink = self.urdfLinks[childLinkIndex] parentLink = self.urdfLinks[parentLinkIndex] joint = UrdfJoint() joint.link = childLink joint.joint_name = "joint_dummy1" joint.joint_type = p.JOINT_REVOLUTE joint.joint_lower_limit = 0 joint.joint_upper_limit = -1 joint.parent_name = parentLink.link_name joint.child_name = childLink.link_name joint.joint_origin_xyz = jointPivotXYZInParent joint.joint_origin_rpy = jointPivotRPYInParent joint.joint_axis_xyz = [0,0,1] #the following commented line would crash PyBullet, it messes up the joint indexing/ordering #self.urdfJoints.append(joint) #so make sure to insert the joint in the right place, to links/joints match self.urdfJoints.insert(insertJointIndex,joint) return joint
def _randomly_place_objects(self, urdfList):
"""Randomly places the objects in the bin.
Args:
urdfList: The list of urdf files to place in the bin.
Returns:
The list of object unique ID's.
"""
# Randomize positions of each object urdf.
objectUids = []
for urdf_name in urdfList:
xpos = 0.4 +self._blockRandom*random.random()
ypos = self._blockRandom*(random.random()-.5)
angle = np.pi/2 + self._blockRandom * np.pi * random.random()
orn = p.getQuaternionFromEuler([0, 0, angle])
urdf_path = os.path.join(self._urdfRoot, urdf_name)
uid = p.loadURDF(urdf_path, [xpos, ypos, .15],
[orn[0], orn[1], orn[2], orn[3]])
objectUids.append(uid)
# Let each object fall to the tray individual, to prevent object
# intersection.
for _ in range(500):
p.stepSimulation()
return objectUids
def _reset(self):
self.terminated = 0
p.resetSimulation()
p.setPhysicsEngineParameter(numSolverIterations=150)
p.setTimeStep(self._timeStep)
p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
xpos = 0.5 +0.2*random.random()
ypos = 0 +0.25*random.random()
ang = 3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
p.setGravity(0,0,-10)
self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
self._envStepCounter = 0
p.stepSimulation()
self._observation = self.getExtendedObservation()
return np.array(self._observation)
#coord = p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
#coordPos = [0,0,0]
#coordOrnEuler = [0,0,0]
#coordPos= [0.7000000000000004, 0, 0.22000000000000006]
#coordOrnEuler= [0, -0.2617993877991496, 0]
coordPos= [0.07, 0, -0.6900000000000004]
coordOrnEuler= [0, 0, 0]
coordPos2= [0, 0, 0 ]
coordOrnEuler2= [0, 0, 0]
invPos,invOrn=p.invertTransform(coordPos,p.getQuaternionFromEuler(coordOrnEuler))
mPos,mOrn = p.multiplyTransforms(invPos,invOrn, coordPos2,p.getQuaternionFromEuler(coordOrnEuler2))
eul = p.getEulerFromQuaternion(mOrn)
print("rpy=\"",eul[0],eul[1],eul[2],"\" xyz=\"", mPos[0],mPos[1], mPos[2])
shift=0
gui = 1
frame=0
states=[]
states.append(p.saveState())
#observations=[]
#observations.append(obs)
def reset(self):
self.setup_timing()
self.task_success = 0
self.forearm_in_sleeve = False
self.upperarm_in_sleeve = False
self.meanft = np.zeros(6)
self.wheelchair = self.world_creation.create_new_world(furniture_type='wheelchair', static_human_base=True, human_impairment='random', gender='random')
if self.robot.wheelchair_mounted:
wheelchair_pos, wheelchair_orient = self.wheelchair.get_base_pos_orient()
self.robot.set_base_pos_orient(wheelchair_pos + np.array(self.robot.toc_base_pos_offset[self.task]), [0, 0, -np.pi/2.0], euler=True)
joints_positions = [(self.human.j_right_elbow, -90), (self.human.j_left_shoulder_x, -80), (self.human.j_left_elbow, -90), (self.human.j_right_hip_x, -90), (self.human.j_right_knee, 80), (self.human.j_left_hip_x, -90), (self.human.j_left_knee, 80)]
self.human.setup_joints(joints_positions, use_static_joints=True, reactive_force=None if self.human_control else 1, reactive_gain=0.01)
shoulder_pos = self.human.get_pos_orient(self.human.left_shoulder)[0]
elbow_pos = self.human.get_pos_orient(self.human.left_elbow)[0]
wrist_pos = self.human.get_pos_orient(self.human.left_wrist)[0]
target_ee_pos = np.array([0.45, -0.3, 1.2]) + self.np_random.uniform(-0.05, 0.05, size=3)
target_ee_orient = np.array(p.getQuaternionFromEuler(np.array(self.robot.toc_ee_orient_rpy[self.task][0]), physicsClientId=self.id))
offset = np.array([0, 0, 0.1])
if self.robot.wheelchair_mounted:
# Use IK to find starting joint angles for mounted robots
self.robot.ik_random_restarts(right=False, target_pos=target_ee_pos, target_orient=target_ee_orient, max_iterations=1000, max_ik_random_restarts=40, success_threshold=0.03, step_sim=True, check_env_collisions=False)
else:
# Use TOC with JLWKI to find an optimal base position for the robot near the person
target_ee_orient_shoulder = np.array(p.getQuaternionFromEuler(np.array(self.robot.toc_ee_orient_rpy[self.task][1]), physicsClientId=self.id))
self.robot.position_robot_toc(self.task, 'left', [(target_ee_pos, target_ee_orient)], [(shoulder_pos+offset, target_ee_orient_shoulder), (elbow_pos+offset, target_ee_orient), (wrist_pos+offset, target_ee_orient)], base_euler_orient=[0, 0, np.pi], step_sim=True, check_env_collisions=False, right_side=False)
# Open gripper to hold the tool
self.robot.set_gripper_open_position(self.robot.left_gripper_indices, self.robot.gripper_pos[self.task], set_instantly=True)
if self.human_control or self.human.impairment == 'tremor':
# Ensure the human arm remains stable while loading the cloth
self.human.control(self.human.controllable_joint_indices, self.human.get_joint_angles(self.human.controllable_joint_indices), 0.05, 1)
self.start_ee_pos, self.start_ee_orient = self.robot.get_pos_orient(self.robot.left_end_effector)
self.cloth_orig_pos = np.array([0.34658437, -0.30296362, 1.20023387])
self.cloth_offset = self.start_ee_pos - self.cloth_orig_pos
# Setup gripper - cloth constraint
# NOTE: Mass of cloth_attachment determines the stiffness of the anchors!
sphere_collision_1, sphere_visual_1 = self.world_creation.create_sphere(radius=0.0001, mass=0.1, pos=[0, 0, 0], visual=True, collision=False, rgba=[0, 0, 0, 0], return_collision_visual=True)
sphere_collision_2, sphere_visual_2 = self.world_creation.create_sphere(radius=0.0001, mass=0.1, pos=[0, 0, 0], visual=True, collision=False, rgba=[0, 0, 0, 0], return_collision_visual=True)
end_effector_attachment = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=sphere_collision_1, baseVisualShapeIndex=sphere_visual_1, basePosition=self.start_ee_pos, baseOrientation=[0, 0, 0, 1], linkMasses=[0.1], linkCollisionShapeIndices=[sphere_collision_2], linkVisualShapeIndices=[sphere_visual_2], linkPositions=[[0, 0, 0]], linkOrientations=[[0, 0, 0, 1]], linkInertialFramePositions=[[0, 0, 0]], linkInertialFrameOrientations=[[0, 0, 0, 1]], linkParentIndices=[0], linkJointTypes=[p.JOINT_REVOLUTE], linkJointAxis=[[1, 1, 1]], linkLowerLimits=[0], linkUpperLimits=[0], useMaximalCoordinates=False, physicsClientId=self.id)
# end_effector_attachment = p.loadURDF(os.path.join(self.world_creation.directory, 'clothing', 'ft_sensor.urdf'), physicsClientId=self.id)
self.end_effector_attachment = Agent()
self.end_effector_attachment.init(end_effector_attachment, self.id, self.np_random, indices=-1)
self.cloth_attachment = self.world_creation.create_sphere(radius=0.0001, mass=1, pos=self.start_ee_pos, visual=True, collision=False, rgba=[0, 0, 0, 0], maximal_coordinates=True)
# self.robot.create_constraint(self.robot.left_end_effector, self.end_effector_attachment, self.end_effector_attachment.base, parent_orient=[0, 0, -np.pi/2.0])
self.end_effector_attachment.create_constraint(0, self.cloth_attachment, self.cloth_attachment.base)
self.end_effector_attachment.enable_force_torque_sensor(0)
# self.cloth_attachment = self.world_creation.create_sphere(radius=0.0001, mass=1, pos=self.start_ee_pos, visual=True, collision=True, rgba=[0, 0, 0, 0], maximal_coordinates=True)
# # self.robot.create_constraint(self.robot.left_end_effector, self.cloth_attachment, self.cloth_attachment.base, parent_pos=[0.1, 0, 0], parent_orient=[0, 0, -np.pi/2.0])
# self.robot.create_constraint(self.robot.left_end_effector, self.cloth_attachment, self.cloth_attachment.base)
# self.robot.enable_force_torque_sensor(self.robot.left_end_effector)
# Load cloth
# self.cloth = p.loadCloth(os.path.join(self.world_creation.directory, 'clothing', 'hospitalgown_reduced.obj'), scale=1.2, mass=0.16, position=np.array([0.4, -0.42, 0.99]) + self.cloth_offset/1.2, orientation=p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id), bodyAnchorId=self.cloth_attachment.body, anchors=[3831, 3558, 1877, 3708, 1974, 2707, 3201, 637, 2405, 3279, 2708, 2592, 2407, 445, 509, 577], collisionMargin=0.04, rgbaColor=np.array([139./256., 195./256., 74./256., 0.6]), rgbaLineColor=np.array([197./256., 225./256., 165./256., 1]), physicsClientId=self.id)
# p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.05, kDG=10, kDF=0.39, kCHR=2.38, kKHR=2.0, kAHR=2.0, piterations=2, physicsClientId=self.id)
# # Points along the opening of sleeve
# self.triangle1_point_indices = [1814, 1869, 1121]
# self.triangle2_point_indices = [2115, 540, 365]
self.cloth = p.loadCloth(os.path.join(self.world_creation.directory, 'clothing', 'hospitalgown_reduced.obj'), scale=1.4, mass=0.16, position=np.array([0.02, -0.38, 0.83]) + self.cloth_offset/1.4, orientation=p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id), bodyAnchorId=self.cloth_attachment.body, anchors=[2087, 3879, 3681, 3682, 2086, 2041, 987, 2042, 2088, 1647, 2332, 719, 1569, 1528, 721], collisionMargin=0.04, rgbaColor=np.array([139./256., 195./256., 74./256., 0.6]), rgbaLineColor=np.array([197./256., 225./256., 165./256., 1]), physicsClientId=self.id)
# p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.005, kDG=10, kDF=0.39, kCHR=1.0, kKHR=1.0, kAHR=1.0, piterations=5, physicsClientId=self.id)
p.clothParams(self.cloth, kLST=0.055, kAST=1.0, kVST=0.5, kDP=0.01, kDG=10, kDF=0.39, kCHR=1.0, kKHR=1.0, kAHR=1.0, piterations=5, physicsClientId=self.id)
# Points along the opening of sleeve
self.triangle1_point_indices = [1180, 2819, 30]
self.triangle2_point_indices = [1322, 13, 696]
# Points along the end of sleeve
# self.triangle1_point_indices = [621, 37, 1008]
# self.triangle2_point_indices = [130, 3908, 2358]
# double m_kLST; // Material: Linear stiffness coefficient [0,1]
# double m_kAST; // Material: Area/Angular stiffness coefficient [0,1]
# double m_kVST; // Material: Volume stiffness coefficient [0,1]
# double m_kVCF; // Velocities correction factor (Baumgarte)
# double m_kDP; // Damping coefficient [0,1]
# double m_kDG; // Drag coefficient [0,+inf]
# double m_kLF; // Lift coefficient [0,+inf]
# double m_kPR; // Pressure coefficient [-inf,+inf]
# double m_kVC; // Volume conversation coefficient [0,+inf]
# double m_kDF; // Dynamic friction coefficient [0,1]
# double m_kMT; // Pose matching coefficient [0,1]
# double m_kCHR; // Rigid contacts hardness [0,1]
# double m_kKHR; // Kinetic contacts hardness [0,1]
# double m_kSHR; // Soft contacts hardness [0,1]
# double m_kAHR; // Anchors hardness [0,1]
# int m_viterations; // Velocities solver iterations
# int m_piterations; // Positions solver iterations
# int m_diterations; // Drift solver iterations
p.setGravity(0, 0, -9.81/2, physicsClientId=self.id) # Let the cloth settle more gently
p.setGravity(0, 0, 0, body=self.robot.body, physicsClientId=self.id)
p.setGravity(0, 0, -1, body=self.human.body, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.cloth_attachment.body, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.end_effector_attachment.body, physicsClientId=self.id)
p.setPhysicsEngineParameter(numSubSteps=8, physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id)
# Wait for the cloth to settle
for _ in range(50):
# Force the end effector attachment to stay at the end effector
self.end_effector_attachment.set_base_pos_orient(self.start_ee_pos, [0, 0, 0, 1])
# self.cloth_attachment.set_base_pos_orient(self.start_ee_pos, [0, 0, 0, 1])
p.stepSimulation(physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
return self._get_obs([0]*6, [0, 0])
global_axis = gl.GLAxisItem()
global_axis.updateGLOptions({'glLineWidth': (4, )})
window.addItem(global_axis)
print("WINDOW not updated")
window.update()
# configure pybullet and load plane.urdf and quadcopter.urdf
physicsClient = p.connect(
p.DIRECT) # pybullet only for computations no visualisation
p.setGravity(0, 0, -gravity)
p.setTimeStep(Tsample_physics)
# disable real-time simulation, we want to step through the
# physics ourselves with p.stepSimulation()
p.setRealTimeSimulation(0)
planeId = p.loadURDF("plane.urdf", [0, 0, 0],
p.getQuaternionFromEuler([0, 0, 0]))
quadcopterId = p.loadURDF("quadrotor.urdf", [0, 0, 1],
p.getQuaternionFromEuler([0, 0, 0]))
# do a few steps to start simulation and let the quadcopter land safely
for i in range(int(2 / Tsample_physics)):
p.stepSimulation()
# create a pyqtgraph mesh from the quadcopter to visualize
# the quadcopter in the 3D pyqtgraph window
quadcopterMesh = bullet2pyqtgraph(quadcopterId)[0]
window.addItem(quadcopterMesh)
window.update()
# Initialize PID controller gains:
Kp = np.zeros(6)
def grasp_primitive(environment, arm, action, xyz, ori, grip, cube_pos,
grasped, lifted, img):
## move above
loc = np.where(img > 0) # find red pixels
if len(loc[0]) == 0: # if there are no red pixels in view
action[0] = 0.0
action[1] = 0.0
action[2] = 0.4
else: # seen red pixels, go to them
y_relative_dir = np.mean(loc[0]) - cam_dims / 2
x_relative_dir = np.mean(loc[1]) - cam_dims / 2
action[0] = xyz[0] + x_relative_dir * 0.0005 #cube_pos[0]
action[1] = xyz[1] - y_relative_dir * 0.0005
y_pixels = loc[1]
x_pixels = loc[0]
line = np.poly1d(np.polyfit(x_pixels, y_pixels,
1)) # fit a line to determine block angle
gradient = line[1] # angle of the block from gripper
print(xyz[2], cube_pos[2])
if ((xyz[0] - cube_pos[0]) < 0.01) and (
(xyz[1] - cube_pos[1]) < 0.01) and (
(xyz[2] - cube_pos[2]) < 0.13) and (abs(gradient) < 0.015):
if xyz[2] > cube_pos[2] + 0.05:
action[2] = xyz[2] - 0.003
else:
action[2] = xyz[2] + 0.003
action[7] = grip * 25 + 0.1 # grip is in 0-0.04 scale
print('grasping')
grasped += 0.5
action[3:7] = p.getQuaternionFromEuler(
[ori[0] - gradient * 0.5, ori[1],
ori[2]]) # adjust gripper angle to suit block
elif ((xyz[0] - cube_pos[0]) < 0.01) and (
(xyz[1] - cube_pos[1]) < 0.01):
#we are close enough to move down
print('moving to grasp', grasped, lifted)
if xyz[2] > cube_pos[2] + 0.2:
action[2] = xyz[2] - (xyz[2] - 0.005)**2
else:
action[2] = xyz[2] - 0.003
# if xyz[2] > cube_pos[2]+0.05:
# action[2] = xyz[2] - 0.003
# else:
# action[2] = xyz[2] + 0.003
action[3:7] = p.getQuaternionFromEuler(
[ori[0] - gradient * 0.5, ori[1],
ori[2]]) # adjust gripper angle to suit block
else:
# go above it
print('moving above')
action[2] = cube_pos[2] + 0.2
# adjust gripper angle to suit block
return action, grasped, lifted
def createMultiBody(self, basePosition=[0,0,0],baseOrientation=[0,0,0,1], physicsClientId=0):
#assume link[0] is base
if (len(self.urdfLinks)==0):
return -1
#for i in range (len(self.urdfLinks)):
# print("link", i, "=",self.urdfLinks[i].link_name)
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
baseMass = base.urdf_inertial.mass
baseCollisionShapeIndex = -1
baseShapeTypeArray=[]
baseRadiusArray=[]
baseHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
meshScaleArray=[]
basePositionsArray=[]
baseOrientationsArray=[]
for v in base.urdf_collision_shapes:
shapeType = v.geom_type
baseShapeTypeArray.append(shapeType)
baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
baseRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
meshScaleArray.append(v.geom_meshscale)
basePositionsArray.append(v.origin_xyz)
orn=p.getQuaternionFromEuler(v.origin_rpy)
baseOrientationsArray.append(orn)
if (len(baseShapeTypeArray)):
#print("fileNameArray=",fileNameArray)
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=meshScaleArray,
collisionFramePositions=basePositionsArray,
collisionFrameOrientations=baseOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = base.urdf_visual_shapes
shapeTypes=[v.geom_type for v in urdfVisuals]
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals]
lengths=[v.geom_length for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals]
visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]
baseVisualShapeIndex = -1
if (len(shapeTypes)):
#print("len(shapeTypes)=",len(shapeTypes))
#print("len(halfExtents)=",len(halfExtents))
#print("len(radii)=",len(radii))
#print("len(lengths)=",len(lengths))
#print("len(fileNames)=",len(fileNames))
#print("len(meshScales)=",len(meshScales))
#print("len(rgbaColors)=",len(rgbaColors))
#print("len(visualFramePositions)=",len(visualFramePositions))
#print("len(visualFrameOrientations)=",len(visualFrameOrientations))
baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes,
halfExtents=halfExtents,radii=radii,lengths=lengths,fileNames=fileNames,
meshScales=meshScales,rgbaColors=rgbaColors,visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,physicsClientId=physicsClientId)
linkMasses=[]
linkCollisionShapeIndices=[]
linkVisualShapeIndices=[]
linkPositions=[]
linkOrientations=[]
linkInertialFramePositions=[]
linkInertialFrameOrientations=[]
linkParentIndices=[]
linkJointTypes=[]
linkJointAxis=[]
for joint in self.urdfJoints:
link = joint.link
linkMass = link.urdf_inertial.mass
linkCollisionShapeIndex=-1
linkVisualShapeIndex=-1
linkPosition=[0,0,0]
linkOrientation=[0,0,0]
linkInertialFramePosition=[0,0,0]
linkInertialFrameOrientation=[0,0,0]
linkParentIndex=self.linkNameToIndex[joint.parent_name]
linkJointType=joint.joint_type
linkJointAx = joint.joint_axis_xyz
linkShapeTypeArray=[]
linkRadiusArray=[]
linkHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
linkMeshScaleArray=[]
linkPositionsArray=[]
linkOrientationsArray=[]
for v in link.urdf_collision_shapes:
shapeType = v.geom_type
linkShapeTypeArray.append(shapeType)
linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
linkRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
linkMeshScaleArray.append(v.geom_meshscale)
linkPositionsArray.append(v.origin_xyz)
linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))
if (len(linkShapeTypeArray)):
linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray,
radii=linkRadiusArray,
halfExtents=linkHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=linkMeshScaleArray,
collisionFramePositions=linkPositionsArray,
collisionFrameOrientations=linkOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = link.urdf_visual_shapes
linkVisualShapeIndex = -1
shapeTypes=[v.geom_type for v in urdfVisuals]
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii=[v.geom_radius for v in urdfVisuals]
lengths=[v.geom_length for v in urdfVisuals]
fileNames=[v.geom_meshfilename for v in urdfVisuals]
meshScales=[v.geom_meshscale for v in urdfVisuals]
rgbaColors=[v.material_rgba for v in urdfVisuals]
visualFramePositions=[v.origin_xyz for v in urdfVisuals]
visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]
if (len(shapeTypes)):
print("fileNames=",fileNames)
linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes,
halfExtents=halfExtents,radii=radii,lengths=lengths,
fileNames=fileNames,meshScales=meshScales,rgbaColors=rgbaColors,
visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,
physicsClientId=physicsClientId)
linkMasses.append(linkMass)
linkCollisionShapeIndices.append(linkCollisionShapeIndex)
linkVisualShapeIndices.append(linkVisualShapeIndex)
linkPositions.append(joint.joint_origin_xyz)
linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
linkParentIndices.append(linkParentIndex)
linkJointTypes.append(joint.joint_type)
linkJointAxis.append(joint.joint_axis_xyz)
obUid = p.createMultiBody(baseMass,\
baseCollisionShapeIndex=baseCollisionShapeIndex,
baseVisualShapeIndex=baseVisualShapeIndex,
basePosition=basePosition,
baseOrientation=baseOrientation,
baseInertialFramePosition=base.urdf_inertial.origin_xyz,
baseInertialFrameOrientation=p.getQuaternionFromEuler(base.urdf_inertial.origin_rpy),
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=linkParentIndices,
linkJointTypes=linkJointTypes,
linkJointAxis=linkJointAxis,
physicsClientId=physicsClientId)
return obUid
import pybullet as p
import time
import math
p.connect(p.GUI)
p.loadURDF("plane.urdf")
cubeId = p.loadURDF("cube_small.urdf",0,0,1)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
print (cid)
print (p.getConstraintUniqueId(0))
prev=[0,0,1]
a=-math.pi
while 1:
a=a+0.01
if (a>math.pi):
a=-math.pi
time.sleep(.01)
p.setGravity(0,0,-10)
pivot=[a,0,1]
orn = p.getQuaternionFromEuler([a,0,0])
p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50)
p.removeConstraint(cid)
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659])
for i in range (p.getNumJoints(husky)):
print(p.getJointInfo(husky,i))
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659)
ob = kukaId
jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
#put kuka on top of husky
cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1])
baseorn = p.getQuaternionFromEuler([3.1415,0,0.3])
baseorn = [0,0,0,1]
#[0, 0, 0.707, 0.707]
#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints!=7):
exit()
#lower limits for null space
ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
#upper limits for null space
ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
#joint ranges for null space
def __init__(
self,
pos=None,
orn=None,
useFixedBase=False,
init_sliders_pose=4 * [
0,
],
):
# Load the robot
if pos is None:
pos = [0.0, 0, 0.40]
if orn is None:
orn = pybullet.getQuaternionFromEuler([0, 0, 0])
pybullet.setAdditionalSearchPath(Solo8Config.meshes_path)
self.urdf_path = Solo8Config.urdf_path
self.robotId = pybullet.loadURDF(
self.urdf_path,
pos,
orn,
flags=pybullet.URDF_USE_INERTIA_FROM_FILE,
useFixedBase=useFixedBase,
)
pybullet.getBasePositionAndOrientation(self.robotId)
# Create the robot wrapper in pinocchio.
self.pin_robot = Solo8Config.buildRobotWrapper()
# Query all the joints.
num_joints = pybullet.getNumJoints(self.robotId)
for ji in range(num_joints):
pybullet.changeDynamics(
self.robotId,
ji,
linearDamping=0.04,
angularDamping=0.04,
restitution=0.0,
lateralFriction=0.5,
)
self.slider_a = pybullet.addUserDebugParameter("a", 0, 1,
init_sliders_pose[0])
self.slider_b = pybullet.addUserDebugParameter("b", 0, 1,
init_sliders_pose[1])
self.slider_c = pybullet.addUserDebugParameter("c", 0, 1,
init_sliders_pose[2])
self.slider_d = pybullet.addUserDebugParameter("d", 0, 1,
init_sliders_pose[3])
self.base_link_name = "base_link"
controlled_joints = []
for leg in ["FL", "FR", "HL", "HR"]:
controlled_joints += [leg + "_HFE", leg + "_KFE"]
self.joint_names = controlled_joints
# Creates the wrapper by calling the super.__init__.
super(Solo8Robot, self).__init__(
self.robotId,
self.pin_robot,
controlled_joints,
["FL_ANKLE", "FR_ANKLE", "HL_ANKLE", "HR_ANKLE"],
)
def reset(self, ret_images=False, ret_dict=False):
self.setup_timing()
self.task_success = 0
self.human, self.wheelchair, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(
furniture_type='wheelchair',
static_human_base=True,
human_impairment='random',
print_joints=False,
gender='random')
self.robot_lower_limits = self.robot_lower_limits[
self.robot_right_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[
self.robot_right_arm_joint_indices]
self.reset_robot_joints()
#
# if self.robot_type == 'jaco':
# wheelchair_pos, wheelchair_orient = p.getBasePositionAndOrientation(self.wheelchair,
# physicsClientId=self.id)
# p.resetBasePositionAndOrientation(self.robot, np.array(wheelchair_pos) + np.array([-0.35, -0.3, 0.3]),
# p.getQuaternionFromEuler([0, 0, -np.pi / 2.0], physicsClientId=self.id),
# physicsClientId=self.id)
# base_pos, base_orient = p.getBasePositionAndOrientation(self.robot, physicsClientId=self.id)
joints_positions = [(6, np.deg2rad(-90)), (16, np.deg2rad(-90)),
(28, np.deg2rad(-90)), (31, np.deg2rad(80)),
(35, np.deg2rad(-90)), (38, np.deg2rad(80))]
joints_positions += [
(21, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))),
(22, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30))),
(23, self.np_random.uniform(np.deg2rad(-30), np.deg2rad(30)))
]
self.human_controllable_joint_indices = [20, 21, 22, 23]
self.world_creation.setup_human_joints(
self.human,
joints_positions,
self.human_controllable_joint_indices if
(self.human_control
or self.world_creation.human_impairment == 'tremor') else [],
use_static_joints=True,
human_reactive_force=None)
p.resetBasePositionAndOrientation(
self.human, [0, 0.03, 0.89 if self.gender == 'male' else 0.86],
[0, 0, 0, 1],
physicsClientId=self.id)
human_joint_states = p.getJointStates(
self.human,
jointIndices=self.human_controllable_joint_indices,
physicsClientId=self.id)
self.target_human_joint_positions = np.array(
[x[0] for x in human_joint_states])
self.human_lower_limits = self.human_lower_limits[
self.human_controllable_joint_indices]
self.human_upper_limits = self.human_upper_limits[
self.human_controllable_joint_indices]
# Place a bowl of food on a table
self.table = p.loadURDF(os.path.join(self.world_creation.directory,
'table', 'table_tall.urdf'),
basePosition=[0.35, -0.9, 0],
baseOrientation=p.getQuaternionFromEuler(
[0, 0, 0], physicsClientId=self.id),
physicsClientId=self.id)
# MOUTH SIM
self.mouth_pos = [-0, -0.15, 1.2]
self.mouth_orient = p.getQuaternionFromEuler(
[np.pi / 2, np.pi / 2, -np.pi / 2], physicsClientId=self.id)
self.mouth = p.loadURDF(os.path.join(self.world_creation.directory,
'mouth', 'hole.urdf'),
useFixedBase=True,
basePosition=self.mouth_pos,
baseOrientation=self.mouth_orient,
flags=p.URDF_USE_SELF_COLLISION,
physicsClientId=self.id)
sph_vis = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.005,
rgbaColor=[0, 1, 0, 1],
physicsClientId=self.id)
self.mouth_center_vis = p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sph_vis,
basePosition=self.mouth_pos,
useMaximalCoordinates=False,
physicsClientId=self.id)
self.batch_ray_offsets = [[0.08, 0, 0], [-0.08, 0, 0], [0, 0, 0.05],
[0, 0, -0.05]]
self.ray_markers = [
p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sph_vis,
basePosition=self.mouth_pos,
useMaximalCoordinates=False,
physicsClientId=self.id) for i in range(4)
]
self.ray_markers_end = [
p.createMultiBody(baseMass=0.0,
baseCollisionShapeIndex=-1,
baseVisualShapeIndex=sph_vis,
basePosition=self.mouth_pos,
useMaximalCoordinates=False,
physicsClientId=self.id) for i in range(4)
]
# SCALE = 0.1
# self.mouthVisualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
# fileName=os.path.join(self.world_creation.directory, 'mouth', 'hole2.obj'),
# rgbaColor=[0.8, 0.4, 0, 1],
# # specularColor=[0.4, .4, 0],
# visualFramePosition=[0, 0, 0],
# meshScale=[SCALE] * 3,
# physicsClientId=self.id)
# self.mouthCollisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH,
# fileName=os.path.join(self.world_creation.directory, 'mouth', 'hole2.obj'),
# collisionFramePosition=[0, 0, 0],
# meshScale=[SCALE] * 3,
# flags=p.GEOM_FORCE_CONCAVE_TRIMESH,
# physicsClientId=self.id)
# self.mouth = p.createMultiBody(baseMass=0.1,
# baseInertialFramePosition=[0, 0, 0],
# baseCollisionShapeIndex=self.mouthCollisionShapeId,
# baseVisualShapeIndex=self.mouthVisualShapeId,
# basePosition=[0, 0, 0],
# useMaximalCoordinates=True,
# flags=p.URDF_USE_SELF_COLLISION,
# physicsClientId=self.id)
p.resetDebugVisualizerCamera(cameraDistance=1.10,
cameraYaw=40,
cameraPitch=-45,
cameraTargetPosition=[-0.2, 0, 0.75],
physicsClientId=self.id)
# ROBOT STUFF
# target_pos = np.array(bowl_pos) + np.array([0, -0.1, 0.4]) + self.np_random.uniform(-0.05, 0.05, size=3)
if self.robot_type == 'panda':
# target_pos = [0.2, -0.7, 1.4]
# target_orient = [-0.7350878727462702, -8.032928869253401e-09, 7.4087721728719465e-09,
# 0.6779718425874067]
# target_orient = [0.4902888273008801, -0.46462044731135954, -0.5293302738768326, -0.5133752690981496]
# target_orient = p.getQuaternionFromEuler(np.array([0, 0, np.pi / 2.0]), physicsClientId=self.id)
# self.util.ik_random_restarts(self.robot, 8, target_pos, target_orient, self.world_creation,
# self.robot_right_arm_joint_indices, self.robot_lower_limits,
# self.robot_upper_limits,
# ik_indices=range(len(self.robot_right_arm_joint_indices)), max_iterations=1000,
# max_ik_random_restarts=40, random_restart_threshold=0.01, step_sim=True,
# check_env_collisions=True)
#
# positions = [0.42092164, -0.92326318, -0.33538581, -2.65185322, 1.40763901, 1.81818155, 0.58610855, 0, 0,
# 0.02, 0.02] # 11
# positions = [0.4721324, -0.87257245, -0.2123101, -2.44757844, 1.47352227, 1.90545005, 0.75139663]
positions = [
0.44594466, -0.58616227, -0.78082232, -2.59866731, 1.15649738,
1.54176148, 0.15145287
]
for idx in range(len(positions)):
p.resetJointState(self.robot, idx, positions[idx])
self.world_creation.set_gripper_open_position(self.robot,
position=0.00,
left=False,
set_instantly=True)
self.drop_fork = self.world_creation.init_tool(
self.robot,
mesh_scale=[0.08] * 3,
pos_offset=[0, 0, 0.08], # fork: [0, -0.02, 0.16],
orient_offset=p.getQuaternionFromEuler(
[-0, -np.pi, 0], physicsClientId=self.id),
left=False,
maximal=False)
else:
raise NotImplementedError
# LOAD FOOD ITEM
self.food_type = np.random.randint(0, len(self.foods), dtype=int)
self.food_scale = np.random.uniform(
self.foods_scale_range[self.food_type][0],
self.foods_scale_range[self.food_type][1])
print("LOADING food file: %s with scale %.2f" %
(self.foods[self.food_type], self.food_scale))
self.child_offset = self.food_scale * self.food_offsets[
self.food_type] # [-0.004, -0.0065]
self.food_orient_eul = np.random.rand(3) * 2 * np.pi
self.food_orient_quat = p.getQuaternionFromEuler(
self.food_orient_eul, physicsClientId=self.id)
mesh_scale = np.array(
[self.food_base_scale[self.food_type]] * 3) * self.food_scale
food_visual = p.createVisualShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(self.world_creation.directory, 'food_items',
self.foods[self.food_type] + ".obj"),
rgbaColor=[1.0, 1.0, 1.0, 1.0],
meshScale=mesh_scale,
physicsClientId=self.id)
food_collision = p.createCollisionShape(
shapeType=p.GEOM_MESH,
fileName=os.path.join(self.world_creation.directory, 'food_items',
self.foods[self.food_type] + "_vhacd.obj"),
meshScale=mesh_scale,
physicsClientId=self.id)
self.food_item = p.createMultiBody(
baseMass=0.012,
baseCollisionShapeIndex=food_collision,
baseVisualShapeIndex=food_visual,
basePosition=[0, 0, 0],
useMaximalCoordinates=False,
physicsClientId=self.id)
p.changeVisualShape(
self.food_item,
-1,
textureUniqueId=p.loadTexture(
os.path.join(self.world_creation.directory, 'food_items',
self.foods[self.food_type] + ".png")))
# Disable collisions between the tool and food item
for ti in list(
range(p.getNumJoints(self.drop_fork,
physicsClientId=self.id))) + [-1]:
for tj in list(
range(
p.getNumJoints(self.food_item,
physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(self.drop_fork,
self.food_item,
ti,
tj,
False,
physicsClientId=self.id)
# Create constraint that keeps the food item in the tool
constraint = p.createConstraint(
self.drop_fork,
-1,
self.food_item,
-1,
p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, -0.025],
childFramePosition=self.child_offset,
parentFrameOrientation=self.food_orient_quat,
childFrameOrientation=[0, 0, 0, 1],
physicsClientId=self.id)
p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
# p.resetBasePositionAndOrientation(self.bowl, bowl_pos, p.getQuaternionFromEuler([np.pi/2.0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.human, physicsClientId=self.id)
p.setPhysicsEngineParameter(numSubSteps=5,
numSolverIterations=10,
physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,
1,
physicsClientId=self.id)
# init_pos = [0.0, 0.0, 0.0, -2 * np.pi / 4, 0.0, np.pi / 2, np.pi / 4, 0.0, 0.0, 0.05, 0.05]
# self._reset_robot(init_pos)
# initialize images
for i in range(10):
p.stepSimulation()
ee_pos, ee_orient = p.getBasePositionAndOrientation(self.food_item)
self.viewMat1 = p.computeViewMatrixFromYawPitchRoll(
ee_pos, self.camdist, self.yaw, self.pitch, self.roll, 2, self.id)
self.viewMat2 = p.computeViewMatrixFromYawPitchRoll(
ee_pos, self.camdist, -self.yaw, self.pitch, self.roll, 2, self.id)
self.projMat = p.computeProjectionMatrixFOV(
self.fov, self.img_width / self.img_height, self.near, self.far)
images1 = p.getCameraImage(self.img_width,
self.img_height,
self.viewMat1,
self.projMat,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
physicsClientId=self.id)
images2 = p.getCameraImage(self.img_width,
self.img_height,
self.viewMat2,
self.projMat,
shadow=True,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
physicsClientId=self.id)
self.rgb_opengl1 = np.reshape(
images1[2], (self.img_width, self.img_height, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images1[3],
[self.img_width, self.img_height])
self.depth_opengl1 = self.far * self.near / (
self.far - (self.far - self.near) * depth_buffer_opengl)
self.rgb_opengl2 = np.reshape(
images2[2], (self.img_width, self.img_height, 4)) * 1. / 255.
depth_buffer_opengl = np.reshape(images2[3],
[self.img_width, self.img_height])
self.depth_opengl2 = self.far * self.near / (
self.far - (self.far - self.near) * depth_buffer_opengl)
if self.gui:
# TODO: do we need to update this every step? prob not
self.im1.set_data(self.rgb_opengl1)
self.im2.set_data(self.rgb_opengl2)
self.im1_d.set_data(self.depth_opengl1)
self.im2_d.set_data(self.depth_opengl2)
print(np.min(self.depth_opengl1), np.max(self.depth_opengl1))
self.fig.canvas.draw()
return self._get_obs(ret_images=ret_images, ret_dict=ret_dict)
def LoadModel(self, model_name, model_path, start_pos = [0, 0, 0], start_orientation = [0, 0, 0]):
start_orientation = p.getQuaternionFromEuler(start_orientation)
self.model_name_id_dict[model_name] = p.loadURDF(model_path, start_pos, start_orientation, useFixedBase=1)
import pybullet as p
import time
import pybullet_data
# Setup environment
physicsClient = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeId = p.loadURDF("plane.urdf")
p.setAdditionalSearchPath("/home/oreo/Documents/oreo_sim/four_bar_link")
p.setRealTimeSimulation(0)
startPos = [0, 0, 1]
startOrn = p.getQuaternionFromEuler([0, 0, 0])
linkage = p.loadURDF("four_bar_link.urdf", startPos, startOrn, useFixedBase=1)
#Set joint control here
numJoints = p.getNumJoints(linkage)
jointInfoList = {}
for i in range(numJoints):
jointInfo = p.getJointInfo(linkage, i)
jointInfoList[jointInfo[1].decode('UTF-8')] = jointInfo[0]
print(jointInfo[0])
# Set some positions
#p.resetJointState(linkage, jointInfoList['joint1'], -1.57)
#p.resetJointState(linkage, jointInfoList['joint2'], -1.57)
#p.resetJointState(linkage, jointInfoList['joint3'], -1.57)
p.setGravity(0, 0, -9.8)
# Constraint to close the kinematic loop
cid = p.createConstraint(linkage, jointInfoList['dummy_joint'], linkage,
def create_smplx_body(self,
directory,
id,
np_random,
gender='female',
height=None,
body_shape=None,
joint_angles=[],
position=[0, 0, 0],
orientation=[0, 0, 0],
body_pose=None):
# Choose gender
self.gender = gender
if self.gender not in ['male', 'female']:
self.gender = np_random.choice(['male', 'female'])
# Create SMPL-X model
model_folder = os.path.join(directory, 'smpl_models')
model = smplx.create(model_folder,
model_type='smplx',
gender=self.gender)
# Define body shape
if type(body_shape) == str:
params_filename = os.path.join(model_folder, 'human_params',
body_shape)
with open(params_filename, 'rb') as f:
params = pickle.load(f)
betas = torch.Tensor(params['betas'])
elif body_shape is None:
betas = torch.Tensor(
np_random.uniform(-1, 5, (1, self.num_body_shape)))
else:
betas = torch.Tensor(body_shape)
# betas = torch.Tensor(np.zeros((1, 10)))
# betas = torch.Tensor(np.zeros((1, 10)))
# Set human body pose
if body_pose is None:
body_pose = np.zeros((1, model.NUM_BODY_JOINTS * 3))
for joint_index, angle in joint_angles:
body_pose[0, joint_index] = np.deg2rad(angle)
# Generate standing human mesh and determine default height of the mesh
output = model(betas=betas,
body_pose=torch.Tensor(
np.zeros((1, model.NUM_BODY_JOINTS * 3))),
return_verts=True)
vertices = output.vertices.detach().cpu().numpy().squeeze()
out_mesh = trimesh.Trimesh(vertices, model.faces)
rot = trimesh.transformations.rotation_matrix(np.deg2rad(90),
[1, 0, 0])
out_mesh.apply_transform(rot)
# Find indices for right arm vertices and save to file
# self.vert_indices = np.where(np.logical_and(np.logical_and(vertices[:, 0] < -0.17, vertices[:, 1] > -0.1), vertices[:, 0] > -0.64))
# self.vert_indices = np.where(np.logical_and(np.logical_and(np.logical_and(np.logical_and(vertices[:, 1] > -0.4, vertices[:, 1] < -0.37), vertices[:, 0] < 0.02), vertices[:, 0] > -0.02), vertices[:, 2] < 0))
# np.savetxt('right_arm_vertex_indices.csv', self.vert_indices, delimiter=',', fm1='%d')
# Generate human mesh with correct height scaling
height_scale = height / out_mesh.extents[
-1] if height is not None else 1.0
# print('Scale:', height_scale, '=', height, '/', out_mesh.extents[-1])
output = model(betas=betas,
body_pose=torch.Tensor(body_pose),
return_verts=True)
vertices = output.vertices.detach().cpu().numpy().squeeze()
joints = output.joints.detach().cpu().numpy().squeeze()
# Scale vertices and rotate
orient_quat = p.getQuaternionFromEuler(orientation, physicsClientId=id)
vertices = vertices * height_scale
vertices = vertices.dot(
R.from_euler('x', -90, degrees=True).as_matrix())
vertices = vertices.dot(R.from_quat(orient_quat).as_matrix())
joints = joints * height_scale
joints = joints.dot(R.from_euler('x', -90, degrees=True).as_matrix())
joints = joints.dot(R.from_quat(orient_quat).as_matrix())
out_mesh = trimesh.Trimesh(vertices, model.faces)
# scale = trimesh.transformations.scale_matrix(height_scale, [0, 0, 0])
# out_mesh.apply_transform(scale)
# rot = trimesh.transformations.rotation_matrix(np.deg2rad(90), [1, 0, 0])
# out_mesh.apply_transform(rot)
return out_mesh, vertices, joints
def move_in_xyz(environment, arm, abs_rel='abs'):
motorsIds = []
dv = 0.01
abs_distance = 1.0
#
environment._arm.resetJointPoses()
observation = environment._arm.getObservation()
xyz = observation[-7:-4]
ori = p.getEulerFromQuaternion(observation[-4:])
if abs_rel == 'abs':
if arm == 'ur5':
xin = xyz[0]
yin = xyz[1]
zin = xyz[2]
rin = -0.6943 #ori[0]
pitchin = 1.587 #ori[1]
yawin = -0.694 # ori[2]
#rin = 1.57#
#pitchin = -0.78 #
#yawin = -0.78
print(pitchin)
print(rin)
print(yawin)
motorsIds.append(
environment._p.addUserDebugParameter("X", -abs_distance,
abs_distance, xin))
motorsIds.append(
environment._p.addUserDebugParameter("Y", -abs_distance,
abs_distance, yin))
motorsIds.append(
environment._p.addUserDebugParameter("Z", -abs_distance,
abs_distance, zin))
motorsIds.append(
environment._p.addUserDebugParameter(
"roll",
-math.pi,
math.pi,
rin,
))
motorsIds.append(
environment._p.addUserDebugParameter("pitch", -math.pi, math.pi,
pitchin))
motorsIds.append(
environment._p.addUserDebugParameter("yaw", -math.pi, math.pi,
yawin))
motorsIds.append(
environment._p.addUserDebugParameter("fingerAngle", 0, 1.5, .3))
grasp_switch = environment._p.addUserDebugParameter("grasp", 0, 2, 0)
throw_switch = environment._p.addUserDebugParameter("throw", 0, 2, 0)
throw_range = environment._p.addUserDebugParameter("range", 0, 5, 1)
done = False
grasped = 0
lifted = 0
throw_timeout = 0
while (not done):
action = []
# get state
observation = environment._arm.getObservation()
grip_img, rgb = gripper_camera(observation)
img = process_image(rgb)
save_image(img)
xyz = observation[-7:-4]
ori = p.getEulerFromQuaternion(observation[-4:])
grip = observation[6]
print(grip)
cube_pos = get_scene_observation(environment.objects)[0:3]
if cube_pos[2] < 0.0: #reset cube if it falls below table.
p.resetBasePositionAndOrientation(environment.objects[0][0],
[0, 0, 0],
[0.0, 0.0, 0.000000, 1.0])
print(grasped, lifted)
if xyz[2] < 0.15:
print('reset lift')
lifted = 0
if not (((xyz[0] - cube_pos[0]) < 0.02) or
((xyz[1] - cube_pos[1]) < 0.02) or
((xyz[2] - cube_pos[2]) < 0.15)):
grasped = 0
for motorId in motorsIds:
# print(environment._p.readUserDebugParameter(motorId))
action.append(environment._p.readUserDebugParameter(motorId))
update_camera(environment)
#environment._p.addUserDebugLine(environment._arm.endEffectorPos, [0, 0, 0], [1, 0, 0], 5)
# action is xyz positon, orietnation quaternion, gripper closedness.
action = action[0:3] + list(p.getQuaternionFromEuler(
action[3:6])) + [action[6]]
if environment._p.readUserDebugParameter(throw_switch) > 1:
action, grasped, lifted, throw_timeout = throw(
environment, arm, action, xyz, ori, grip, cube_pos, grasped,
lifted, throw_timeout, img,
environment._p.readUserDebugParameter(throw_range))
elif environment._p.readUserDebugParameter(grasp_switch) > 1:
action, grasped, lifted = grasp_primitive(environment, arm, action,
xyz, ori, grip, cube_pos,
grasped, lifted, img)
state, motor_action, reward, done, info = environment.step_to(
action, abs_rel)
obs = environment.getSceneObservation()
TORSO_JOINT_NAME = 'torso_lift_joint'
#Setup node
pub = rospy.Publisher('pybullet_cmds', uc_msg, queue_size=0)
rospy.init_node("pybullet_test")
rospy.loginfo("testing pybullet configurations")
#Setup controller
uc = Uber()
uc.command_joint_pose('l', [0]*7, time=2, blocking=True)
#Setup sim
physicsClient = connect(use_gui=True) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath())
pr2_start_orientation = p.getQuaternionFromEuler([0,0,0])
pr2_start_pose = [-.80*k,0,0]
pr2 = p.loadURDF("/home/demo/nishad/pddlstream/examples/pybullet/utils/models/pr2_description/pr2.urdf", pr2_start_pose, pr2_start_orientation,
useFixedBase=True, globalScaling = 1 )
#The goal here is to get the arm positions of the actual robot using the uber controller
#and simulate them in pybullet
print str(uc.get_head_pose()) + ','
#Left Manipulator
left_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['left']]
l_joint_poses = uc.get_joint_positions('l')
#Right Manipulator
right_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['right']]
r_joint_poses = uc.get_joint_positions('r')
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0):
#assume link[0] is base
if (len(self.urdfLinks)==0):
return -1
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
baseMass = base.urdf_inertial.mass
print("baseMass=",baseMass)
baseCollisionShapeIndex = -1
baseShapeTypeArray=[]
baseRadiusArray=[]
baseHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
meshScaleArray=[]
basePositionsArray=[]
baseOrientationsArray=[]
for v in base.urdf_collision_shapes:
print("base v.origin_xyz=",v.origin_xyz)
print("base v.origin_rpy=",v.origin_rpy)
shapeType = v.geom_type
baseShapeTypeArray.append(shapeType)
baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
baseRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
meshScaleArray.append(v.geom_meshscale)
basePositionsArray.append(v.origin_xyz)
print("v.origin_rpy=",v.origin_rpy)
orn=p.getQuaternionFromEuler(v.origin_rpy)
baseOrientationsArray.append(orn)
print("baseHalfExtentsArray=",baseHalfExtentsArray)
if (len(baseShapeTypeArray)):
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=meshScaleArray,
collisionFramePositions=basePositionsArray,
collisionFrameOrientations=baseOrientationsArray,
physicsClientId=physicsClientId)
print("baseCollisionShapeIndex=",baseCollisionShapeIndex)
linkMasses=[]
linkCollisionShapeIndices=[]
linkVisualShapeIndices=[]
linkPositions=[]
linkOrientations=[]
linkMeshScaleArray=[]
linkInertialFramePositions=[]
linkInertialFrameOrientations=[]
linkParentIndices=[]
linkJointTypes=[]
linkJointAxis=[]
for joint in self.urdfJoints:
link = joint.link
linkMass = link.urdf_inertial.mass
print("linkMass=",linkMass)
linkCollisionShapeIndex=-1
linkVisualShapeIndex=-1
linkPosition=[0,0,0]
linkOrientation=[0,0,0]
linkInertialFramePosition=[0,0,0]
linkInertialFrameOrientation=[0,0,0]
linkParentIndex=self.linkNameToIndex[joint.parent_name]
print("parentId=",linkParentIndex)
linkJointType=joint.joint_type
print("linkJointType=",linkJointType, self.jointTypeToName[linkJointType] )
linkJointAx = joint.joint_axis_xyz
linkShapeTypeArray=[]
linkRadiusArray=[]
linkHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
linkPositionsArray=[]
linkOrientationsArray=[]
for v in link.urdf_collision_shapes:
print("link v.origin_xyz=",v.origin_xyz)
print("link v.origin_rpy=",v.origin_rpy)
shapeType = v.geom_type
linkShapeTypeArray.append(shapeType)
linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
linkRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
linkMeshScaleArray.append(v.geom_meshscale)
linkPositionsArray.append(v.origin_xyz)
linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))
print("linkHalfExtentsArray=",linkHalfExtentsArray)
if (len(linkShapeTypeArray)):
linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray,
radii=linkRadiusArray,
halfExtents=linkHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=linkMeshScaleArray,
collisionFramePositions=linkPositionsArray,
collisionFrameOrientations=linkOrientationsArray,
physicsClientId=physicsClientId)
linkMasses.append(linkMass)
linkCollisionShapeIndices.append(linkCollisionShapeIndex)
linkVisualShapeIndices.append(linkVisualShapeIndex)
linkPositions.append(joint.joint_origin_xyz)
linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
linkParentIndices.append(linkParentIndex)
linkJointTypes.append(joint.joint_type)
linkJointAxis.append(joint.joint_axis_xyz)
print("\n\n\nlinkMasses=",linkMasses)
obUid = p.createMultiBody(baseMass,\
baseCollisionShapeIndex= baseCollisionShapeIndex,
basePosition=basePosition,
baseInertialFramePosition=base.urdf_inertial.origin_xyz,
baseInertialFrameOrientation=base.urdf_inertial.origin_rpy,
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=linkParentIndices,
linkJointTypes=linkJointTypes,
linkJointAxis=linkJointAxis,
physicsClientId=physicsClientId)
return obUid
def configure_simulation(dt, enableGUI):
global jointTorques
# Load robot
robot = loadTalos()
robot.initDisplay(loadModel=True)
rmodel = robot.model
# Defining the initial state of the robot
q0 = robot.model.referenceConfigurations['half_sitting'].copy()
q0[2] = 1.2 # Note that pinocchio's index is as follow: left leg (7), right leg, torso(2), left arm(8), right arm, head(2)
v0 = pinocchio.utils.zero(robot.model.nv)
nq = robot.model.nq
nv = robot.model.nv
na = nq - 7 # actual activated joint
# Start the client for PyBullet
if enableGUI:
physicsClient = p.connect(p.GUI)
else:
physicsClient = p.connect(p.DIRECT) # noqa
# p.GUI for graphical version
# p.DIRECT for non-graphical version
# Set gravity (disabled by default)
p.setGravity(0, 0, -9.81)
# Load horizontal plane for PyBullet
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("plane.urdf")
# Load the robot for PyBullet
robotStartPos = q0[:3]
robotStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
p.setAdditionalSearchPath(
"/opt/openrobots/share/example-robot-data/robots/talos_data/robots")
robotId = p.loadURDF("talos_reduced.urdf", robotStartPos,
robotStartOrientation)
# Set time step of the simulation
# dt = 0.001
p.setTimeStep(dt)
# realTimeSimulation = True # If True then we will sleep in the main loop to have a frequency of 1/dt
# set Initial Posture
revoluteJointIndices = np.concatenate(
(np.arange(45, 51), np.arange(52, 58), np.arange(
0, 2), np.arange(11, 19), np.arange(28, 36), np.arange(3, 5)),
axis=None)
for m in range(na):
p.resetJointState(robotId, revoluteJointIndices[m], q0[m + 7])
# Disable default motor control for revolute joints
p.setJointMotorControlArray(
robotId,
jointIndices=revoluteJointIndices,
controlMode=p.VELOCITY_CONTROL,
targetVelocities=[0.0 for m in revoluteJointIndices],
forces=[0.0 for m in revoluteJointIndices])
# Enable torque control for revolute joints
jointTorques = [0.0 for m in revoluteJointIndices]
p.setJointMotorControlArray(robotId,
revoluteJointIndices,
controlMode=p.TORQUE_CONTROL,
forces=jointTorques)
# Compute one step of simulation for initialization
p.stepSimulation()
return robotId, robot, revoluteJointIndices
#print("duration=",frameData[0])
#print(pos=[frameData])
basePos1Start = [frameData[1],frameData[2],frameData[3]]
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
#pre-rotate to make z-up
if (useZUp):
y2zPos=[0,0,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
y2zPos=[0,2,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn)
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
def reset(self):
self.setup_timing()
self.task_success = 0
self.forearm_in_sleeve = False
self.upperarm_in_sleeve = False
self.human, self.wheelchair, self.robot, self.robot_lower_limits, self.robot_upper_limits, self.human_lower_limits, self.human_upper_limits, self.robot_right_arm_joint_indices, self.robot_left_arm_joint_indices, self.gender = self.world_creation.create_new_world(furniture_type='wheelchair', static_human_base=True, human_impairment='random', print_joints=False, gender='random')
self.robot_lower_limits = self.robot_lower_limits[self.robot_left_arm_joint_indices]
self.robot_upper_limits = self.robot_upper_limits[self.robot_left_arm_joint_indices]
self.reset_robot_joints()
if self.robot_type == 'jaco':
wheelchair_pos, wheelchair_orient = p.getBasePositionAndOrientation(self.wheelchair, physicsClientId=self.id)
p.resetBasePositionAndOrientation(self.robot, np.array(wheelchair_pos) + np.array([0.35, -0.3, 0.3]), p.getQuaternionFromEuler([0, 0, 0], physicsClientId=self.id), physicsClientId=self.id)
joints_positions = [(6, np.deg2rad(-90)), (13, np.deg2rad(-80)), (16, np.deg2rad(-90)), (28, np.deg2rad(-90)), (31, np.deg2rad(80)), (35, np.deg2rad(-90)), (38, np.deg2rad(80))]
self.human_controllable_joint_indices = [10, 11, 12, 13, 14, 15, 16, 17, 18, 19]
self.world_creation.setup_human_joints(self.human, joints_positions, self.human_controllable_joint_indices if (self.human_control or self.world_creation.human_impairment == 'tremor') else [], use_static_joints=True, human_reactive_force=None)
p.resetBasePositionAndOrientation(self.human, [0, 0.03, 0.89 if self.gender == 'male' else 0.86], [0, 0, 0, 1], physicsClientId=self.id)
human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
self.target_human_joint_positions = np.array([x[0] for x in human_joint_states])
self.human_lower_limits = self.human_lower_limits[self.human_controllable_joint_indices]
self.human_upper_limits = self.human_upper_limits[self.human_controllable_joint_indices]
shoulder_pos, shoulder_orient = p.getLinkState(self.human, 15, computeForwardKinematics=True, physicsClientId=self.id)[:2]
elbow_pos, elbow_orient = p.getLinkState(self.human, 17, computeForwardKinematics=True, physicsClientId=self.id)[:2]
wrist_pos, wrist_orient = p.getLinkState(self.human, 19, computeForwardKinematics=True, physicsClientId=self.id)[:2]
target_pos = np.array([-0.85, -0.4, 0]) + np.array([1.85, 0.6, 0]) + np.array([-0.55, -0.5, 1.2]) + self.np_random.uniform(-0.05, 0.05, size=3)
offset = np.array([0, 0, 0.1])
if self.robot_type == 'pr2':
target_orient = p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id)
target_orient_shoulder = p.getQuaternionFromEuler([0, 0, np.pi*3/2.0], physicsClientId=self.id)
self.position_robot_toc(self.robot, 76, [(target_pos, target_orient)], [(shoulder_pos+offset, target_orient_shoulder), (elbow_pos+offset, target_orient), (wrist_pos+offset, target_orient)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=range(29, 29+7), pos_offset=np.array([1.7, 0.7, 0]), base_euler_orient=[0, 0, np.pi], right_side=False, max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions)
elif self.robot_type == 'jaco':
target_orient = p.getQuaternionFromEuler(np.array([0, -np.pi/2.0, 0]), physicsClientId=self.id)
target_joint_positions = self.util.ik_random_restarts(self.robot, 8, target_pos, target_orient, self.world_creation, self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=[0, 1, 2, 3, 4, 5, 6], max_iterations=1000, max_ik_random_restarts=40, random_restart_threshold=0.03, step_sim=True)
self.world_creation.set_gripper_open_position(self.robot, position=1.33, left=False, set_instantly=True)
else:
target_orient = p.getQuaternionFromEuler([0, -np.pi/2.0, 0], physicsClientId=self.id)
target_orient_shoulder = p.getQuaternionFromEuler([np.pi/2.0, -np.pi/2.0, 0], physicsClientId=self.id)
if self.robot_type == 'baxter':
self.position_robot_toc(self.robot, 48, [(target_pos, target_orient)], [(shoulder_pos+offset, target_orient_shoulder), (elbow_pos+offset, target_orient), (wrist_pos+offset, target_orient)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=range(10, 17), pos_offset=np.array([1.7, 0.7, 0.975]), base_euler_orient=[0, 0, np.pi], right_side=False, max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions)
else:
self.position_robot_toc(self.robot, 19, [(target_pos, target_orient)], [(shoulder_pos+offset, target_orient_shoulder), (elbow_pos+offset, target_orient), (wrist_pos+offset, target_orient)], self.robot_left_arm_joint_indices, self.robot_lower_limits, self.robot_upper_limits, ik_indices=[0, 2, 3, 4, 5, 6, 7], pos_offset=np.array([1.8, 0.7, 0.975]), base_euler_orient=[0, 0, np.pi], right_side=False, max_ik_iterations=200, step_sim=True, check_env_collisions=False, human_joint_indices=self.human_controllable_joint_indices, human_joint_positions=self.target_human_joint_positions)
if self.human_control or self.world_creation.human_impairment == 'tremor':
human_len = len(self.human_controllable_joint_indices)
human_joint_states = p.getJointStates(self.human, jointIndices=self.human_controllable_joint_indices, physicsClientId=self.id)
human_joint_positions = np.array([x[0] for x in human_joint_states])
p.setJointMotorControlArray(self.human, jointIndices=self.human_controllable_joint_indices, controlMode=p.POSITION_CONTROL, targetPositions=human_joint_positions, positionGains=np.array([0.005]*human_len), forces=[1]*human_len, physicsClientId=self.id)
state = p.getLinkState(self.robot, 76 if self.robot_type=='pr2' else 19 if self.robot_type=='sawyer' else 48 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)
self.start_ee_pos = np.array(state[0])
self.start_ee_orient = np.array(state[1]) # Quaternions
self.cloth_orig_pos = np.array([0.34658437, -0.30296362, 1.20023387])
self.cloth_offset = self.start_ee_pos - self.cloth_orig_pos
# Set up gripper - cloth constraint
gripper_visual = p.createVisualShape(p.GEOM_SPHERE, radius=0.01, rgbaColor=[0, 0, 0, 0], physicsClientId=self.id)
gripper_collision = p.createCollisionShape(p.GEOM_SPHERE, radius=0.0001, physicsClientId=self.id)
self.cloth_attachment = p.createMultiBody(baseMass=0, baseVisualShapeIndex=gripper_visual, baseCollisionShapeIndex=gripper_collision, basePosition=self.start_ee_pos, useMaximalCoordinates=1, physicsClientId=self.id)
# Load cloth
self.cloth = p.loadCloth(os.path.join(self.world_creation.directory, 'clothing', 'hospitalgown_reduced.obj'), scale=1.4, mass=0.23, position=np.array([0.02, -0.38, 0.83]) + self.cloth_offset/1.4, orientation=p.getQuaternionFromEuler([0, 0, np.pi], physicsClientId=self.id), bodyAnchorId=self.cloth_attachment, anchors=[2087, 3879, 3681, 3682, 2086, 2041, 987, 2042, 2088, 1647, 2332], collisionMargin=0.04, rgbaColor=np.array([139./256., 195./256., 74./256., 0.6]), rgbaLineColor=np.array([197./256., 225./256., 165./256., 1]), physicsClientId=self.id)
p.clothParams(self.cloth, kLST=0.05, kAST=1.0, kVST=1.0, kDP=0.001, kDG=10, kDF=0.25, kCHR=1.0, kKHR=1.0, kAHR=0.5, piterations=5, physicsClientId=self.id)
self.triangle1_point_indices = [621, 37, 1008]
self.triangle2_point_indices = [130, 3908, 2358]
# double m_kLST; // Material: Linear stiffness coefficient [0,1]
# double m_kAST; // Material: Area/Angular stiffness coefficient [0,1]
# double m_kVST; // Material: Volume stiffness coefficient [0,1]
# double m_kVCF; // Velocities correction factor (Baumgarte)
# double m_kDP; // Damping coefficient [0,1]
# double m_kDG; // Drag coefficient [0,+inf]
# double m_kLF; // Lift coefficient [0,+inf]
# double m_kPR; // Pressure coefficient [-inf,+inf]
# double m_kVC; // Volume conversation coefficient [0,+inf]
# double m_kDF; // Dynamic friction coefficient [0,1]
# double m_kMT; // Pose matching coefficient [0,1]
# double m_kCHR; // Rigid contacts hardness [0,1]
# double m_kKHR; // Kinetic contacts hardness [0,1]
# double m_kSHR; // Soft contacts hardness [0,1]
# double m_kAHR; // Anchors hardness [0,1]
# int m_viterations; // Velocities solver iterations
# int m_piterations; // Positions solver iterations
# int m_diterations; // Drift solver iterations
p.setGravity(0, 0, -9.81/2, physicsClientId=self.id) # Let the cloth settle more gently
p.setGravity(0, 0, 0, body=self.robot, physicsClientId=self.id)
p.setGravity(0, 0, -1, body=self.human, physicsClientId=self.id)
p.setGravity(0, 0, 0, body=self.cloth_attachment, physicsClientId=self.id)
p.setPhysicsEngineParameter(numSubSteps=4, physicsClientId=self.id)
# Enable rendering
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1, physicsClientId=self.id)
# Wait for the cloth to settle
for _ in range(200):
# Force the cloth attachment to stay at the end effector
p.resetBasePositionAndOrientation(self.cloth_attachment, self.start_ee_pos, [0, 0, 0, 1], physicsClientId=self.id)
p.stepSimulation(physicsClientId=self.id)
p.setGravity(0, 0, -9.81, physicsClientId=self.id)
return self._get_obs([0], [0, 0])
def applyAction(self, motorCommands):
#print ("self.numJoints")
#print (self.numJoints)
if (self.useInverseKinematics):
dx = motorCommands[0]
dy = motorCommands[1]
dz = motorCommands[2]
da = motorCommands[3]
fingerAngle = motorCommands[4]
state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
#print("pos[2] (getLinkState(kukaEndEffectorIndex)")
#print(actualEndEffectorPos[2])
self.endEffectorPos[0] = self.endEffectorPos[0]+dx
if (self.endEffectorPos[0]>0.65):
self.endEffectorPos[0]=0.65
if (self.endEffectorPos[0]<0.50):
self.endEffectorPos[0]=0.50
self.endEffectorPos[1] = self.endEffectorPos[1]+dy
if (self.endEffectorPos[1]<-0.17):
self.endEffectorPos[1]=-0.17
if (self.endEffectorPos[1]>0.22):
self.endEffectorPos[1]=0.22
#print ("self.endEffectorPos[2]")
#print (self.endEffectorPos[2])
#print("actualEndEffectorPos[2]")
#print(actualEndEffectorPos[2])
#if (dz<0 or actualEndEffectorPos[2]<0.5):
self.endEffectorPos[2] = self.endEffectorPos[2]+dz
self.endEffectorAngle = self.endEffectorAngle + da
pos = self.endEffectorPos
orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw])
if (self.useNullSpace==1):
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp)
else:
if (self.useOrientation==1):
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd)
else:
jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos)
#print("jointPoses")
#print(jointPoses)
#print("self.kukaEndEffectorIndex")
#print(self.kukaEndEffectorIndex)
if (self.useSimulation):
for i in range (self.kukaEndEffectorIndex+1):
#print(i)
p.setJointMotorControl2(bodyUniqueId=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,maxVelocity=self.maxVelocity, positionGain=0.3,velocityGain=1)
else:
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range (self.numJoints):
p.resetJointState(self.kukaUid,i,jointPoses[i])
#fingers
p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce)
p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce)
p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce)
p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
else:
for action in range (len(motorCommands)):
motor = self.motorIndices[action]
p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)
import pybullet as p
from time import sleep
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
useRealTimeSimulation = 0
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
p.setGravity(0,0,-10)
sleep(0.01) # Time in seconds.
else:
p.stepSimulation()
def transform_orientation(self, orientation):
A = np.quaternion(*orientation)
B = np.quaternion(*p.getQuaternionFromEuler([0, np.pi / 2, 0]))
C = B * A
return quaternion.as_float_array(C)
import pybullet as p
import time
useMaximalCoordinates = 0
p.connect(p.GUI)
#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
orn = p.getQuaternionFromEuler([1.5707963,0,0])
p.createMultiBody (0,monastryId, baseOrientation=orn)
sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
mass = 1
visualShapeId = -1
for i in range (5):
for j in range (5):
for k in range (5):
if (k&2):
sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
else:
sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
p.setGravity(0,0,-10)
objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
hand=objects[0]
ho = p.getQuaternionFromEuler([3.14,-3.14/2,0])
hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[-0.05,0,0.02],[0.500000,0.300006,0.700000],ho)
print ("hand_cid")
print (hand_cid)
for i in range (p.getNumJoints(hand)):
p.setJointMotorControl2(hand,i,p.POSITION_CONTROL,0,0)
#clamp in range 400-600
#minV = 400
#maxV = 600
minV = 250
maxV = 450
POSITION=1
def applyAction(self, motorCommands):
# print ("self.numJoints")
# print (self.numJoints)
if (self.useInverseKinematics):
dx = motorCommands[0]
dy = motorCommands[1]
dz = motorCommands[2]
da = motorCommands[3]
fingerAngle = motorCommands[4]
state = p.getLinkState(self.kukaUid, self.kukaEndEffectorIndex)
actualEndEffectorPos = state[0]
# print("pos[2] (getLinkState(kukaEndEffectorIndex)")
# print(actualEndEffectorPos[2])
self.endEffectorPos[0] = self.endEffectorPos[0] + dx
if (self.endEffectorPos[0] > 0.65):
self.endEffectorPos[0] = 0.65
if (self.endEffectorPos[0] < 0.50):
self.endEffectorPos[0] = 0.50
self.endEffectorPos[1] = self.endEffectorPos[1] + dy
if (self.endEffectorPos[1] < -0.17):
self.endEffectorPos[1] = -0.17
if (self.endEffectorPos[1] > 0.22):
self.endEffectorPos[1] = 0.22
# print ("self.endEffectorPos[2]")
# print (self.endEffectorPos[2])
# print("actualEndEffectorPos[2]")
# print(actualEndEffectorPos[2])
# if (dz<0 or actualEndEffectorPos[2]<0.5):
self.endEffectorPos[2] = self.endEffectorPos[2] + dz
self.endEffectorAngle = self.endEffectorAngle + da
pos = self.endEffectorPos
orn = p.getQuaternionFromEuler([0, -math.pi, 0]) # -math.pi,yaw])
if (self.useNullSpace == 1):
if (self.useOrientation == 1):
jointPoses = p.calculateInverseKinematics(
self.kukaUid, self.kukaEndEffectorIndex, pos, orn,
self.ll, self.ul, self.jr, self.rp)
else:
jointPoses = p.calculateInverseKinematics(
self.kukaUid,
self.kukaEndEffectorIndex,
pos,
lowerLimits=self.ll,
upperLimits=self.ul,
jointRanges=self.jr,
restPoses=self.rp)
else:
if (self.useOrientation == 1):
jointPoses = p.calculateInverseKinematics(
self.kukaUid,
self.kukaEndEffectorIndex,
pos,
orn,
jointDamping=self.jd) # todo make damping list bigger
else:
jointPoses = p.calculateInverseKinematics(
self.kukaUid, self.kukaEndEffectorIndex, pos)
# print("jointPoses")
# print(jointPoses)
# print("self.kukaEndEffectorIndex")
# print(self.kukaEndEffectorIndex)
if (self.useSimulation):
for i in range(self.kukaEndEffectorIndex + 1):
# print(i)
p.setJointMotorControl2(bodyUniqueId=self.kukaUid,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=self.maxForce,
maxVelocity=self.maxVelocity,
positionGain=0.3,
velocityGain=1)
else:
# reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(self.numJoints):
p.resetJointState(self.kukaUid, i, jointPoses[i])
# fingers
p.setJointMotorControl2(self.kukaUid,
7,
p.POSITION_CONTROL,
targetPosition=self.endEffectorAngle,
force=self.maxForce)
p.setJointMotorControl2(self.kukaUid,
8,
p.POSITION_CONTROL,
targetPosition=-fingerAngle,
force=self.fingerAForce)
p.setJointMotorControl2(self.kukaUid,
11,
p.POSITION_CONTROL,
targetPosition=fingerAngle,
force=self.fingerBForce)
p.setJointMotorControl2(self.kukaUid,
10,
p.POSITION_CONTROL,
targetPosition=0,
force=self.fingerTipForce)
p.setJointMotorControl2(self.kukaUid,
13,
p.POSITION_CONTROL,
targetPosition=0,
force=self.fingerTipForce)
else:
for action in range(len(motorCommands)):
motor = self.motorIndices[action]
p.setJointMotorControl2(self.kukaUid,
motor,
p.POSITION_CONTROL,
targetPosition=motorCommands[action],
force=self.maxForce)
targetVelocity = 4
maxForce = 2.5
# maxForce = 0
# targetVelocity = 0
# maxForce = 25
all_wheels = [20, 21]
all_wheels = [15, 16]
# import pdb;
# pdb.set_trace()
for wheel in all_wheels:
p.setJointMotorControl2(self.kukaUid,
wheel,
p.VELOCITY_CONTROL,
targetVelocity=targetVelocity,
force=maxForce)
def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0):
#assume link[0] is base
if (len(self.urdfLinks)==0):
return -1
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
baseMass = base.urdf_inertial.mass
baseCollisionShapeIndex = -1
baseShapeTypeArray=[]
baseRadiusArray=[]
baseHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
meshScaleArray=[]
basePositionsArray=[]
baseOrientationsArray=[]
for v in base.urdf_collision_shapes:
shapeType = v.geom_type
baseShapeTypeArray.append(shapeType)
baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
baseRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
meshScaleArray.append(v.geom_meshscale)
basePositionsArray.append(v.origin_xyz)
orn=p.getQuaternionFromEuler(v.origin_rpy)
baseOrientationsArray.append(orn)
if (len(baseShapeTypeArray)):
baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=meshScaleArray,
collisionFramePositions=basePositionsArray,
collisionFrameOrientations=baseOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = base.urdf_visual_shapes
baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals],
radii=[v.geom_radius for v in urdfVisuals],
lengths=[v.geom_length[0] for v in urdfVisuals],
fileNames=[v.geom_meshfilename for v in urdfVisuals],
meshScales=[v.geom_meshscale for v in urdfVisuals],
rgbaColors=[v.material_rgba for v in urdfVisuals],
visualFramePositions=[v.origin_xyz for v in urdfVisuals],
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals],
physicsClientId=physicsClientId)
# urdfVisual = base.urdf_visual_shapes[0]
# baseVisualShapeIndex = p.createVisualShape(shapeType=urdfVisual.geom_type,
# halfExtents=[ext * 0.5 for ext in urdfVisual.geom_extents],
# radius=urdfVisual.geom_radius,
# length=urdfVisual.geom_length[0],
# fileName=urdfVisual.geom_meshfilename,
# meshScale=urdfVisual.geom_meshscale,
# rgbaColor=urdfVisual.material_rgba,
# visualFramePosition=urdfVisual.origin_xyz,
# visualFrameOrientation=urdfVisual.origin_rpy,
# physicsClientId=physicsClientId)
linkMasses=[]
linkCollisionShapeIndices=[]
linkVisualShapeIndices=[]
linkPositions=[]
linkOrientations=[]
linkMeshScaleArray=[]
linkInertialFramePositions=[]
linkInertialFrameOrientations=[]
linkParentIndices=[]
linkJointTypes=[]
linkJointAxis=[]
for joint in self.urdfJoints:
link = joint.link
linkMass = link.urdf_inertial.mass
linkCollisionShapeIndex=-1
linkVisualShapeIndex=-1
linkPosition=[0,0,0]
linkOrientation=[0,0,0]
linkInertialFramePosition=[0,0,0]
linkInertialFrameOrientation=[0,0,0]
linkParentIndex=self.linkNameToIndex[joint.parent_name]
linkJointType=joint.joint_type
linkJointAx = joint.joint_axis_xyz
linkShapeTypeArray=[]
linkRadiusArray=[]
linkHalfExtentsArray=[]
lengthsArray=[]
fileNameArray=[]
linkPositionsArray=[]
linkOrientationsArray=[]
for v in link.urdf_collision_shapes:
shapeType = v.geom_type
linkShapeTypeArray.append(shapeType)
linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
linkRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
linkMeshScaleArray.append(v.geom_meshscale)
linkPositionsArray.append(v.origin_xyz)
linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))
if (len(linkShapeTypeArray)):
linkCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=linkShapeTypeArray,
radii=linkRadiusArray,
halfExtents=linkHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=linkMeshScaleArray,
collisionFramePositions=linkPositionsArray,
collisionFrameOrientations=linkOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = link.urdf_visual_shapes
linkVisualShapeIndex = p.createVisualShapeArray(shapeTypes=[v.geom_type for v in urdfVisuals],
halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals],
radii=[v.geom_radius for v in urdfVisuals],
lengths=[v.geom_length[0] for v in urdfVisuals],
fileNames=[v.geom_meshfilename for v in urdfVisuals],
meshScales=[v.geom_meshscale for v in urdfVisuals],
rgbaColors=[v.material_rgba for v in urdfVisuals],
visualFramePositions=[v.origin_xyz for v in urdfVisuals],
visualFrameOrientations=[v.origin_rpy for v in urdfVisuals],
physicsClientId=physicsClientId)
linkMasses.append(linkMass)
linkCollisionShapeIndices.append(linkCollisionShapeIndex)
linkVisualShapeIndices.append(linkVisualShapeIndex)
linkPositions.append(joint.joint_origin_xyz)
linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
linkParentIndices.append(linkParentIndex)
linkJointTypes.append(joint.joint_type)
linkJointAxis.append(joint.joint_axis_xyz)
obUid = p.createMultiBody(baseMass,\
baseCollisionShapeIndex=baseCollisionShapeIndex,
baseVisualShapeIndex=baseVisualShapeIndex,
basePosition=basePosition,
baseInertialFramePosition=base.urdf_inertial.origin_xyz,
baseInertialFrameOrientation=base.urdf_inertial.origin_rpy,
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=linkParentIndices,
linkJointTypes=linkJointTypes,
linkJointAxis=linkJointAxis,
physicsClientId=physicsClientId)
return obUid
def joinUrdf(self,
childEditor,
parentLinkIndex=0,
jointPivotXYZInParent=[0, 0, 0],
jointPivotRPYInParent=[0, 0, 0],
jointPivotXYZInChild=[0, 0, 0],
jointPivotRPYInChild=[0, 0, 0],
parentPhysicsClientId=0,
childPhysicsClientId=0):
childLinkIndex = len(self.urdfLinks)
insertJointIndex = len(self.urdfJoints)
#combine all links, and add a joint
for link in childEditor.urdfLinks:
self.linkNameToIndex[link.link_name] = len(self.urdfLinks)
self.urdfLinks.append(link)
for joint in childEditor.urdfJoints:
self.urdfJoints.append(joint)
#add a new joint between a particular
jointPivotQuatInChild = p.getQuaternionFromEuler(jointPivotRPYInChild)
invJointPivotXYZInChild, invJointPivotQuatInChild = p.invertTransform(
jointPivotXYZInChild, jointPivotQuatInChild)
#apply this invJointPivot***InChild to all inertial, visual and collision element in the child link
#inertial
pos, orn = p.multiplyTransforms(self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,
p.getQuaternionFromEuler(
self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),
invJointPivotXYZInChild,
invJointPivotQuatInChild,
physicsClientId=parentPhysicsClientId)
self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos
self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p.getEulerFromQuaternion(orn)
#all visual
for v in self.urdfLinks[childLinkIndex].urdf_visual_shapes:
pos, orn = p.multiplyTransforms(v.origin_xyz, p.getQuaternionFromEuler(v.origin_rpy),
invJointPivotXYZInChild, invJointPivotQuatInChild)
v.origin_xyz = pos
v.origin_rpy = p.getEulerFromQuaternion(orn)
#all collision
for c in self.urdfLinks[childLinkIndex].urdf_collision_shapes:
pos, orn = p.multiplyTransforms(c.origin_xyz, p.getQuaternionFromEuler(c.origin_rpy),
invJointPivotXYZInChild, invJointPivotQuatInChild)
c.origin_xyz = pos
c.origin_rpy = p.getEulerFromQuaternion(orn)
childLink = self.urdfLinks[childLinkIndex]
parentLink = self.urdfLinks[parentLinkIndex]
joint = UrdfJoint()
joint.link = childLink
joint.joint_name = "joint_dummy1"
joint.joint_type = p.JOINT_REVOLUTE
joint.joint_lower_limit = 0
joint.joint_upper_limit = -1
joint.parent_name = parentLink.link_name
joint.child_name = childLink.link_name
joint.joint_origin_xyz = jointPivotXYZInParent
joint.joint_origin_rpy = jointPivotRPYInParent
joint.joint_axis_xyz = [0, 0, 1]
#the following commented line would crash PyBullet, it messes up the joint indexing/ordering
#self.urdfJoints.append(joint)
#so make sure to insert the joint in the right place, to links/joints match
self.urdfJoints.insert(insertJointIndex, joint)
return joint
def main():
pygame.init()
screen = pygame.display.set_mode((800, 480),
pygame.OPENGL | pygame.DOUBLEBUF)
glEnable(GL_DEPTH_TEST)
shaderProgram = shaders.compileProgram(
shaders.compileShader(vertexShader, GL_VERTEX_SHADER),
shaders.compileShader(fragmentShader, GL_FRAGMENT_SHADER))
boxPos = [0.0, 0.0, 5.0]
boxOrn = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
planePos = [0.0, 0.0, 0.0]
planeOrn = p.getQuaternionFromEuler([0.0, 0.0, 0.0])
physicsClientId = initPyBullet()
objectManager = URDFManager(physicsClientId)
planeId = objectManager.add("data/plane.urdf", planePos, planeOrn)
boxId = objectManager.add("data/007-box.urdf", boxPos, boxOrn)
shapes = objectManager.getVisualShapes(planeId)
for key, visualShape in shapes.items():
visualShape.vs.prepare(shaderProgram)
shapes = objectManager.getVisualShapes(boxId)
for key, visualShape in shapes.items():
visualShape.vs.prepare(shaderProgram)
print("----------")
print(p.getNumJoints(boxId))
print(p.getJointInfo(boxId, 0))
"""
for i in range(p.getNumJoints(r2d2)):
jointInfo = p.getJointInfo(r2d2, i)
print("-------"+str(i)+"-------")
print(jointInfo)
"""
#GEOM_SPHERE, GEOM_BOX, GEOM_CAPSULE, GEOM_CYLINDER, GEOM_PLANE, GEOM_MESH
"""
visualShapes = p.getVisualShapeData(r2d2)
i = 0
for vs in visualShapes:
print("-------"+str(i)+"-------")
print(vs)
i += 1
"""
clock = pygame.time.Clock()
done = False
tick = 0
velocity = 0.0
force = 0.0
while not done:
for event in pygame.event.get():
if event.type == pygame.QUIT:
done = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_UP:
velocity = -100
force = 250.0
elif event.key == pygame.K_DOWN:
velocity = 100
force = 250.0
elif event.type == pygame.KEYUP:
if event.key == pygame.K_UP or event.key == pygame.K_DOWN:
velocity = 0.0
force = 0.0
p.setJointMotorControl2(bodyUniqueId=boxId,
jointIndex=0,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=velocity,
force=force)
tick += 1
startDisplay()
shapes = objectManager.getVisualShapes(planeId)
for key, visualShape in shapes.items():
#visualShape.updatePosAndOrn(planePos, planeOrn)
visualShape.vs.draw(shaderProgram)
quatBoxPos, quatBoxOrn = p.getBasePositionAndOrientation(boxId)
boxPos = TranslationMatrix(quatBoxPos)
boxOrn = getOrnMatrixFromQuaternion(quatBoxOrn)
shapes = objectManager.getVisualShapes(boxId)
for key, visualShape in shapes.items():
if (key != -1):
linkState = p.getLinkState(boxId, key)
quatLinkPos = linkState[0]
quatLinkOrn = linkState[1]
linkPos = TranslationMatrix(quatLinkPos)
linkOrn = getOrnMatrixFromQuaternion(quatLinkOrn)
else:
linkPos = boxPos
linkOrn = boxOrn
pos = linkPos
orn = visualShape.orn @ linkOrn
visualShape.vs.updatePosAndOrn(pos, orn)
visualShape.vs.draw(shaderProgram)
endDisplay()
p.stepSimulation()
pygame.display.flip()
clock.tick(60)
def createMultiBody(self,
basePosition=[0, 0, 0],
baseOrientation=[0, 0, 0, 1],
physicsClientId=0):
#assume link[0] is base
if (len(self.urdfLinks) == 0):
return -1
#for i in range (len(self.urdfLinks)):
# print("link", i, "=",self.urdfLinks[i].link_name)
base = self.urdfLinks[0]
#v.tmp_collision_shape_ids=[]
baseMass = base.urdf_inertial.mass
baseCollisionShapeIndex = -1
baseShapeTypeArray = []
baseRadiusArray = []
baseHalfExtentsArray = []
lengthsArray = []
fileNameArray = []
meshScaleArray = []
basePositionsArray = []
baseOrientationsArray = []
for v in base.urdf_collision_shapes:
shapeType = v.geom_type
baseShapeTypeArray.append(shapeType)
baseHalfExtentsArray.append(
[0.5 * v.geom_extents[0], 0.5 * v.geom_extents[1], 0.5 * v.geom_extents[2]])
baseRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
meshScaleArray.append(v.geom_meshscale)
basePositionsArray.append(v.origin_xyz)
orn = p.getQuaternionFromEuler(v.origin_rpy)
baseOrientationsArray.append(orn)
if (len(baseShapeTypeArray)):
#print("fileNameArray=",fileNameArray)
baseCollisionShapeIndex = p.createCollisionShapeArray(
shapeTypes=baseShapeTypeArray,
radii=baseRadiusArray,
halfExtents=baseHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=meshScaleArray,
collisionFramePositions=basePositionsArray,
collisionFrameOrientations=baseOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = base.urdf_visual_shapes
shapeTypes = [v.geom_type for v in urdfVisuals]
halfExtents = [[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii = [v.geom_radius for v in urdfVisuals]
lengths = [v.geom_length for v in urdfVisuals]
fileNames = [v.geom_meshfilename for v in urdfVisuals]
meshScales = [v.geom_meshscale for v in urdfVisuals]
rgbaColors = [v.material_rgba for v in urdfVisuals]
visualFramePositions = [v.origin_xyz for v in urdfVisuals]
visualFrameOrientations = [p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]
baseVisualShapeIndex = -1
if (len(shapeTypes)):
#print("len(shapeTypes)=",len(shapeTypes))
#print("len(halfExtents)=",len(halfExtents))
#print("len(radii)=",len(radii))
#print("len(lengths)=",len(lengths))
#print("len(fileNames)=",len(fileNames))
#print("len(meshScales)=",len(meshScales))
#print("len(rgbaColors)=",len(rgbaColors))
#print("len(visualFramePositions)=",len(visualFramePositions))
#print("len(visualFrameOrientations)=",len(visualFrameOrientations))
baseVisualShapeIndex = p.createVisualShapeArray(
shapeTypes=shapeTypes,
halfExtents=halfExtents,
radii=radii,
lengths=lengths,
fileNames=fileNames,
meshScales=meshScales,
rgbaColors=rgbaColors,
visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,
physicsClientId=physicsClientId)
linkMasses = []
linkCollisionShapeIndices = []
linkVisualShapeIndices = []
linkPositions = []
linkOrientations = []
linkInertialFramePositions = []
linkInertialFrameOrientations = []
linkParentIndices = []
linkJointTypes = []
linkJointAxis = []
for joint in self.urdfJoints:
link = joint.link
linkMass = link.urdf_inertial.mass
linkCollisionShapeIndex = -1
linkVisualShapeIndex = -1
linkPosition = [0, 0, 0]
linkOrientation = [0, 0, 0]
linkInertialFramePosition = [0, 0, 0]
linkInertialFrameOrientation = [0, 0, 0]
linkParentIndex = self.linkNameToIndex[joint.parent_name]
linkJointType = joint.joint_type
linkJointAx = joint.joint_axis_xyz
linkShapeTypeArray = []
linkRadiusArray = []
linkHalfExtentsArray = []
lengthsArray = []
fileNameArray = []
linkMeshScaleArray = []
linkPositionsArray = []
linkOrientationsArray = []
for v in link.urdf_collision_shapes:
shapeType = v.geom_type
linkShapeTypeArray.append(shapeType)
linkHalfExtentsArray.append(
[0.5 * v.geom_extents[0], 0.5 * v.geom_extents[1], 0.5 * v.geom_extents[2]])
linkRadiusArray.append(v.geom_radius)
lengthsArray.append(v.geom_length)
fileNameArray.append(v.geom_meshfilename)
linkMeshScaleArray.append(v.geom_meshscale)
linkPositionsArray.append(v.origin_xyz)
linkOrientationsArray.append(p.getQuaternionFromEuler(v.origin_rpy))
if (len(linkShapeTypeArray)):
linkCollisionShapeIndex = p.createCollisionShapeArray(
shapeTypes=linkShapeTypeArray,
radii=linkRadiusArray,
halfExtents=linkHalfExtentsArray,
lengths=lengthsArray,
fileNames=fileNameArray,
meshScales=linkMeshScaleArray,
collisionFramePositions=linkPositionsArray,
collisionFrameOrientations=linkOrientationsArray,
physicsClientId=physicsClientId)
urdfVisuals = link.urdf_visual_shapes
linkVisualShapeIndex = -1
shapeTypes = [v.geom_type for v in urdfVisuals]
halfExtents = [[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals]
radii = [v.geom_radius for v in urdfVisuals]
lengths = [v.geom_length for v in urdfVisuals]
fileNames = [v.geom_meshfilename for v in urdfVisuals]
meshScales = [v.geom_meshscale for v in urdfVisuals]
rgbaColors = [v.material_rgba for v in urdfVisuals]
visualFramePositions = [v.origin_xyz for v in urdfVisuals]
visualFrameOrientations = [p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]
if (len(shapeTypes)):
print("fileNames=", fileNames)
linkVisualShapeIndex = p.createVisualShapeArray(
shapeTypes=shapeTypes,
halfExtents=halfExtents,
radii=radii,
lengths=lengths,
fileNames=fileNames,
meshScales=meshScales,
rgbaColors=rgbaColors,
visualFramePositions=visualFramePositions,
visualFrameOrientations=visualFrameOrientations,
physicsClientId=physicsClientId)
linkMasses.append(linkMass)
linkCollisionShapeIndices.append(linkCollisionShapeIndex)
linkVisualShapeIndices.append(linkVisualShapeIndex)
linkPositions.append(joint.joint_origin_xyz)
linkOrientations.append(p.getQuaternionFromEuler(joint.joint_origin_rpy))
linkInertialFramePositions.append(link.urdf_inertial.origin_xyz)
linkInertialFrameOrientations.append(p.getQuaternionFromEuler(link.urdf_inertial.origin_rpy))
linkParentIndices.append(linkParentIndex)
linkJointTypes.append(joint.joint_type)
linkJointAxis.append(joint.joint_axis_xyz)
obUid = p.createMultiBody(baseMass,\
baseCollisionShapeIndex=baseCollisionShapeIndex,
baseVisualShapeIndex=baseVisualShapeIndex,
basePosition=basePosition,
baseOrientation=baseOrientation,
baseInertialFramePosition=base.urdf_inertial.origin_xyz,
baseInertialFrameOrientation=p.getQuaternionFromEuler(base.urdf_inertial.origin_rpy),
linkMasses=linkMasses,
linkCollisionShapeIndices=linkCollisionShapeIndices,
linkVisualShapeIndices=linkVisualShapeIndices,
linkPositions=linkPositions,
linkOrientations=linkOrientations,
linkInertialFramePositions=linkInertialFramePositions,
linkInertialFrameOrientations=linkInertialFrameOrientations,
linkParentIndices=linkParentIndices,
linkJointTypes=linkJointTypes,
linkJointAxis=linkJointAxis,
physicsClientId=physicsClientId)
return obUid
amplitude = 0.8
jump_amp = 0.5
maxForce = 3.5
kneeFrictionForce = 0
kp = 1
kd = .5
maxKneeForce = 1000
# physId = p.connect(p.SHARED_MEMORY_GUI)
if (1):
p.connect(p.GUI)
p.resetSimulation()
p.setAdditionalSearchPath(pybullet_data.getDataPath())
angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
# p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
# "genericlogdata.bin",
# maxLogDof=16,
# logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000)
p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
orn,
def __init__(self,
gui,
urdf_path=None,
foot_link_names=[],
terrain_height=0,
field=False,
joints_ft=False,
robot="wolfgang",
load_robot=True):
self.gui = gui
self.paused = False
self.gravity = True
self.terrain_height = terrain_height
self.field_on = field
self.urdf_path = urdf_path
self.foot_link_names = foot_link_names
self.joints_ft = joints_ft
self.robot_type = robot
self.load_robot = load_robot
self.robot_indexes = []
self.joints = {}
self.joint_stall_torques = {}
self.joint_max_vels = {}
self.link_masses = {}
self.link_inertias = {}
self.pressure_sensors = {}
self.links = {}
self.torso_ids = {}
self.last_step_time = 0
self.realtime = False
self.time_multiplier = 0
# config values
self.start_position = [0, 0, 0.43]
self.start_orientation = p.getQuaternionFromEuler((0, 0.25, 0))
if self.robot_type is None:
# no robot to initialize
pass
elif self.robot_type == "wolfgang":
self.initial_joint_positions = {
"LAnklePitch": -30,
"LAnkleRoll": 0,
"LHipPitch": 30,
"LHipRoll": 0,
"LHipYaw": 0,
"LKnee": 60,
"RAnklePitch": 30,
"RAnkleRoll": 0,
"RHipPitch": -30,
"RHipRoll": 0,
"RHipYaw": 0,
"RKnee": -60,
"LShoulderPitch": 75,
"LShoulderRoll": 0,
"LElbow": 36,
"RShoulderPitch": -75,
"RShoulderRoll": 0,
"RElbow": -36,
"HeadPan": 0,
"HeadTilt": 0
}
elif self.robot_type in ["op2", "robotis_op2"]:
self.initial_joint_positions = {
"l_ankle_pitch": 0,
"l_ankle_roll": 0,
"l_hip_pitch": 0,
"l_hip_roll": 0,
"l_hip_yaw": 0,
"l_knee": 0,
"r_ankle_pitch": 0,
"r_ankle_roll": 0,
"r_hip_pitch": 0,
"r_hip_roll": 0,
"r_hip_yaw": 0,
"r_knee": 0,
"l_sho_pitch": 0,
"l_sho_roll": 0,
"LElbow": 0,
"r_sho_pitch": 0,
"r_sho_roll": 0,
"RElbow": 0,
"head_pan": 0,
"head_tilt": 0
}
self.foot_link_names = ["l_foot", "r_foot"]
elif self.robot_type == "sigmaban":
self.start_orientation = p.getQuaternionFromEuler((0, 0.0, 0))
self.initial_joint_positions = {
"left_ankle_pitch": 0,
"left_ankle_roll": 0,
"left_hip_pitch": 0,
"left_hip_roll": 0,
"left_hip_yaw": 0,
"left_knee": 0,
"right_ankle_pitch": 0,
"right_ankle_roll": 0,
"right_hip_pitch": 0,
"right_hip_roll": 0,
"right_hip_yaw": 0,
"right_knee": 0,
"left_shoulder_pitch": 0,
"left_shoulder_roll": 0,
"LElbow": 0,
"right_shoulder_pitch": 0,
"right_shoulder_roll": 0,
"RElbow": 0,
"head_yaw": 0,
"head_pitch": 0
}
else:
print(f"robot {self.robot_type} not known")
quit(0)
# Instantiating Bullet
if self.gui:
self.client_id = p.connect(p.GUI)
else:
self.client_id = p.connect(p.DIRECT)
if self.gravity:
p.setGravity(0, 0, -9.81)
self.time = 0
# disable debug interface, only show robot
p.configureDebugVisualizer(p.COV_ENABLE_GUI, False)
# Engine parameters
# time step should be at 240Hz (due to pyBullet documentation)
self.timestep = 1 / 240
# standard parameters seem to be best. leave them like they are
# p.setPhysicsEngineParameter(fixedTimeStep=self.timestep, numSubSteps=1)
# no real time, as we will publish own clock, but we have option to switch to real_time by waiting
p.setRealTimeSimulation(0)
self.last_wall_time = time()
self.load_models()
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
p.connect(p.GUI)
q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True)
rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)
while True:
roll = p.readUserDebugParameter(rollId)
pitch = p.readUserDebugParameter(pitchId)
yaw = p.readUserDebugParameter(yawId)
x = p.readUserDebugParameter(fwdxId)
y = p.readUserDebugParameter(fwdyId)
z = p.readUserDebugParameter(fwdzId)
orn = p.getQuaternionFromEuler([roll, pitch, yaw])
p.resetBasePositionAndOrientation(q, [x, y, z], orn)
#p.stepSimulation()#not really necessary for this demo, no physics used
import pickle as pk
import numpy as np
import sys
sys.path.append('../../envs')
import pybullet as pb
from humanoid import PoppyHumanoidEnv, convert_angles
env = PoppyHumanoidEnv(pb.POSITION_CONTROL)
N = len(env.joint_index)
with open("../../../scripts/stand.pkl", "rb") as f:
stand_dict = pk.load(f)
stand = env.angle_array(stand_dict)
env.set_position(stand)
pb.resetBasePositionAndOrientation(env.robot_id, (0, .5, 1),
pb.getQuaternionFromEuler((0, 0, 0)))
input('.')
speed = 10
amplitude = 0.8
jump_amp = 0.5
maxForce = 3.5
kneeFrictionForce = 0
kp = 1
kd = .5
maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY)
if (physId < 0):
p.connect(p.GUI)
#p.resetSimulation()
angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
"genericlogdata.bin",
maxLogDof=16,
logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000)
p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
orn,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
return images
if __name__ == "__main__":
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.GUI)
#p.connect(p.SHARED_MEMORY_GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("checkerboard/calibration.urdf", [0.5, 0, 0.01])
p.loadURDF("plane.urdf", [0, 0, 0.0])
d435Id = p.loadURDF("d435/d435.urdf", [0, 0, 0.0])
p.resetBasePositionAndOrientation(
d435Id, [1, 0, 1], p.getQuaternionFromEuler([0, -math.pi / 4, 0]))
pandaId = p.loadURDF("Panda/panda.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(pandaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = 7
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/joint_states_desired", JointState, callback)
pub = rospy.Publisher("point_cloud2", PointCloud2, queue_size=2)
count = 0
while 1:
#print("joint state : ",joint_states)
d435pos, d435orn = p.getBasePositionAndOrientation(d435Id)
d435orn = p.getEulerFromQuaternion(d435orn)
for i in range(numJoints):
p.resetJointState(pandaId, i, joint_states[i])
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
#Joint damping coefficents. Using large values for the joints that we don't want to move.
jd = [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 0.5]
#jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]
p.setGravity(0, 0, 0)
while 1:
p.stepSimulation()
for i in range(1):
pos = [0, 0, 1.26]
orn = p.getQuaternionFromEuler([0, 0, 3.14])
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
def estimate_pose(self,
start_pos,
z_distance=2,
start_orientation=(0, 0, 0)):
start_time = datetime.datetime.now()
start_quat_orientation = p.getQuaternionFromEuler(start_orientation)
start_pos = [start_pos[0], start_pos[1], start_pos[2] + z_distance]
boxId = p.loadURDF(self.urdf_path, start_pos, start_quat_orientation)
linkId = 1
jointFrictionForce = 0
for joint in range(p.getNumJoints(boxId)):
p.setJointMotorControl2(boxId,
joint,
p.POSITION_CONTROL,
force=jointFrictionForce)
linkStateFull = p.getLinkState(boxId, linkId)
prevCubePos, prevCubeOrn = linkStateFull[4], linkStateFull[5]
for i in range(1000):
p.stepSimulation()
linkStateFull = p.getLinkState(boxId, linkId)
cubePos, cubeOrn = linkStateFull[4], linkStateFull[5]
dist = math.sqrt((cubePos[2] - prevCubePos[2])**2) # only z
q0 = pyquaternion.Quaternion(x=cubeOrn[0],
y=cubeOrn[1],
z=cubeOrn[2],
w=cubeOrn[3])
q1 = pyquaternion.Quaternion(x=prevCubeOrn[0],
y=prevCubeOrn[1],
z=prevCubeOrn[2],
w=prevCubeOrn[3])
quat_dist = pyquaternion.Quaternion.absolute_distance(q0, q1)
# print "dist:{:.8f}".format(dist), "quat_dist:{:.6f}".format(quat_dist)
prevCubePos, prevCubeOrn = cubePos, cubeOrn
if i > 10 and (dist <= 0.00001 and quat_dist <= 0.0001):
#print "breaking at step:", i, dist, quat_dist
break
# time.sleep(1./360.)
end_time = datetime.datetime.now()
delta = end_time - start_time
delta_millis = int(delta.total_seconds() * 1000) # milliseconds
#print "total time millis:{}".format(delta_millis)
p.removeBody(boxId)
euler_q1 = p.getEulerFromQuaternion(prevCubeOrn)
R = mesh_helper.Rxyz(euler_q1[0], euler_q1[1], euler_q1[2])
zf = np.array([0, 0, 1])
zf_l = np.dot(R, zf)
return prevCubePos, zf_l
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName="marble_cube.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[1, 1, 1],
visualFramePosition=shift,
meshScale=meshScale)
#collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="textures/marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale)
collisionShapeId = -1
uiCube = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 1, 0],
useMaximalCoordinates=True)
textOrn = p.getQuaternionFromEuler([0, 0, -1.5707963])
numLines = 1
lines = [-1] * numLines
p.stepSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
#objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
kitchenObj = p.loadSDF("kitchens/1.sdf")
#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [
p.loadURDF("pr2_gripper.urdf", 0.500000, 0.300006, 0.700000, -0.000000, -0.000000, -0.000031,
1.000000)
]
pr2_gripper = objects[0]
print("pr2_gripper=")
physicsClient = p.connect(p.GUI) # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally
p.resetDebugVisualizerCamera(cameraDistance=0.45,
cameraYaw=135,
cameraPitch=-45,
cameraTargetPosition=[0, 0, 0])
p.setGravity(0, 0, -10)
frequency = 100 # Hz
p.setTimeStep(1 / frequency)
p.setRealTimeSimulation(0)
planeId = p.loadURDF(URDF(get_scene("plane-big.urdf.xml")).get_path())
startPos = [0, 0, 0] # RGB = xyz
startOrientation = p.getQuaternionFromEuler(
[0, 0, 0]) # rotated around which axis? # np.deg2rad(90)
# rotating a standing cylinder around the y axis, puts it flat onto the x axis
robot = p.loadURDF("urdfs/PoppyErgoJr_pen.urdf.xml",
startPos,
startOrientation,
useFixedBase=1)
for i in range(p.getNumJoints(robot)):
print(p.getJointInfo(robot, i))
motors = [0, 1, 2, 3, 4, 5]
#
debugParams = []
for i in range(len(motors)):
#first try to connect to shared memory (VR), if it fails use local GUI
c = p.connect(p.SHARED_MEMORY)
if (c < 0):
c = p.connect(p.GUI)
#p.resetSimulation()
p.setGravity(0, 0, -10)
print(c)
if (c < 0):
p.connect(p.GUI)
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
hand = objects[0]
ho = p.getQuaternionFromEuler([0, 3.14, 0])
hand_cid = p.createConstraint(hand, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.1, 0, 0],
[0.500000, 0.300006, 0.700000], ho)
print("hand_cid")
print(hand_cid)
for i in range(p.getNumJoints(hand)):
p.setJointMotorControl2(hand, i, p.POSITION_CONTROL, 0, 0)
#clamp in range 400-600
minV = 400
maxV = 600
POSITION = 1
ORIENTATION = 2
BUTTONS = 6
# plane est le sol il est contenu dans l'import pybullet_data
pybuplane = pybullet.loadURDF("plane.urdf")
#os.system("git clone https://github.com/Chris-Annin/ROS_AR2_urdf.git ar2")
#pybullet.setGravity(0,0,-9.8)
# on récpère le bras robot depuiq le dossier git qu'on a récupéré
# c'est important de lui mettre une useFixedBase à 1 pour qu'il ne tombe pas (avec la gravité par exemple)
robot = pybullet.loadURDF("ar2/urdf/ar2.urdf", [0, 0, 0], useFixedBase=1)
# importation du socle
socle = pybullet.loadURDF("socle.urdf", [0, -0.5, 0], useFixedBase=1)
# pour faire l'orientation du cub
orientation = pybullet.getQuaternionFromEuler([73, 0, 0])
# importation du cube
cube = pybullet.loadURDF("cube.urdf", [0.047, -0.46, 0.07], orientation)
pybullet.resetDebugVisualizerCamera(1.40, -53.0, -39.0, (0.53, 0.21, -0.24))
numJoints = pybullet.getNumJoints(robot)
time.sleep(2)
pybullet.setTimeStep(0.0001)
pybullet.setRealTimeSimulation(0)
steps = 400
fixedTimeStep = 1.5 / steps
robotEndEffectorIndex = numJoints - 1
pybullet.setGravity(0, 0, 0)
t = 0.
import pybullet as p
import time
p.connect(p.GUI)
useCollisionShapeQuery = True
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2])
baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0]) #[0,0.5,0.5,0]
basePositionB = [1.5, 0, 1]
obA = -1
obB = -1
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geomBox,
basePosition=basePositionB,
baseOrientation=baseOrientationB)
lineWidth = 3
colorRGB = [1, 0, 0]
lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
lineToXYZ=[0, 0, 0],
lineColorRGB=colorRGB,
lineWidth=lineWidth,
lifeTime=0)
pitch = 0
yaw = 0
while (p.isConnected()):
pitch += 0.01
if (pitch >= 3.1415 * 2.):
def init(self):
if (self._game_settings['render']):
# self._object.setPosition([self._x[self._step], self._y[self._step], 0.0] )
self._physicsClient = p.connect(p.GUI)
else:
self._physicsClient = p.connect(p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
#p.setRealTimeSimulation(True)
p.setGravity(0,0,self._GRAVITY)
p.setTimeStep(self._dt)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,0.5]
cubeStartOrientation = p.getQuaternionFromEuler([0.,0,0])
self._agent = p.loadURDF("sphere2.urdf",
cubeStartPos,
cubeStartOrientation)
self._blocks = []
self._num_blocks=0
for i in range(self._num_blocks):
blockId = p.loadURDF("cube2.urdf",
[2.0,2.0,0.5],
cubeStartOrientation,
useFixedBase=1)
self._blocks.append(blockId)
self._target = p.loadURDF("sphere2red.urdf",
cubeStartPos,
cubeStartOrientation,
useFixedBase=1)
#disable the default velocity motors
#and set some position control with small force to emulate joint friction/return to a rest pose
jointFrictionForce=1
for joint in range (p.getNumJoints(self._agent)):
p.setJointMotorControl2(self._agent,joint,p.POSITION_CONTROL,force=jointFrictionForce)
#for i in range(10000):
# p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0)
# p.stepSimulation()
#import ipdb
#ipdb.set_trace()
p.setRealTimeSimulation(1)
lo = [0.0 for l in self.getObservation()[0]]
hi = [1.0 for l in self.getObservation()[0]]
state_bounds_llc = [lo, hi]
lo = [0.0 for l in self.getObservation()[1]]
hi = [1.0 for l in self.getObservation()[1]]
state_bounds_hlc = [lo, hi]
state_bounds = [state_bounds_llc, state_bounds_hlc]
print ("NavGameHRL2D state bounds: ", state_bounds)
self._game_settings['state_bounds'] = [lo, hi]
self._state_length = len(self._game_settings['state_bounds'][0])
print ("self._state_length: ", self._state_length)
self._observation_space = ActionSpace(self._game_settings['state_bounds'])
self._last_state = self.getState()
self._last_pose = p.getBasePositionAndOrientation(self._agent)[0]
kd = .1
physId = p.connect(p.SHARED_MEMORY)
if (physId<0):
p.connect(p.GUI)
#p.resetSimulation()
p.loadURDF("plane.urdf",0,0,0)
p.setPhysicsEngineParameter(numSolverIterations=50)
p.setTimeOut(4)
p.setGravity(0,0,0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0,0,0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False)
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
0.000000, 0.707107, 0.707107)
]
objects = [
p.loadURDF("cube_small.urdf", 0.950000, -0.100000, 0.700000, 0.000000,
0.000000, 0.707107, 0.707107)
]
objects = [
p.loadURDF("sphere_small.urdf", 0.850000, -0.400000, 0.700000, 0.000000,
0.000000, 0.707107, 0.707107)
]
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")
hand = objects[0]
ho = p.getQuaternionFromEuler([3.14, -3.14 / 2, 0])
hand_cid = p.createConstraint(hand, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0],
[-0.05, 0, 0.02], [0.500000, 0.300006, 0.700000],
ho)
print("hand_cid")
print(hand_cid)
for i in range(p.getNumJoints(hand)):
p.setJointMotorControl2(hand, i, p.POSITION_CONTROL, 0, 0)
#clamp in range 400-600
#minV = 400
#maxV = 600
minV = 250
maxV = 450
POSITION = 1
i = 4
p.setJointMotorControl2(kuka_gripper,
i,
p.POSITION_CONTROL,
targetPosition=e[ANALOG] * 0.05,
force=10)
i = 6
p.setJointMotorControl2(kuka_gripper,
i,
p.POSITION_CONTROL,
targetPosition=e[ANALOG] * 0.05,
force=10)
if sq_len < THRESHOLD * THRESHOLD:
eef_pos = e[POSITION]
eef_orn = p.getQuaternionFromEuler([0, -math.pi, 0])
joint_pos = p.calculateInverseKinematics(kuka,
6,
eef_pos,
eef_orn,
lowerLimits=LOWER_LIMITS,
upperLimits=UPPER_LIMITS,
jointRanges=JOINT_RANGE,
restPoses=REST_POSE,
jointDamping=JOINT_DAMP)
for i in range(len(joint_pos)):
p.setJointMotorControl2(kuka,
i,
p.POSITION_CONTROL,
targetPosition=joint_pos[i],
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=standstilltorque)
p.setJointMotorControl2(bodyIndex=robot,
jointIndex=spring_motor_id,
controlMode=p.VELOCITY_CONTROL,
targetVelocity=0,
force=standstilltorque)
physicsClient = p.connect(p.GUI) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
robotStartPos = [0, 0, 0.23]
robotStartOrientation = p.getQuaternionFromEuler([np.pi / 2, 0, 0])
robot = p.loadURDF("/home/pramod/Single_leg_test/Urdf/Test_1/urdf/Test_1.urdf",
robotStartPos,
robotStartOrientation,
useFixedBase=1)
# ResetLeg()
a = 0.06
b = 0.03
x_path = []
z_path = []
y_path = []
for j in range(0, 361, 5):
# Ellipse
x = -0.06 + a * m.cos(m.radians(j))
z = 0.035 + b * m.sin(m.radians(j))
p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json")
jran = 50
iran = 100
num=64
radius=0.1
numDominoes=0
for i in range (int(num*50)):
num=(radius*2*math.pi)/0.08
radius += 0.05/float(num)
orn = p.getQuaternionFromEuler([0,0,0.5*math.pi+math.pi*2*i/float(num)])
pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03]
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
numDominoes+=1
pos=[pos[0],pos[1],pos[2]+0.3]
orn = p.getQuaternionFromEuler([0,0,-math.pi/4.])
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
print("numDominoes=",numDominoes)
#for j in range (20):
# for i in range (100):
# if (i<99):
# sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
def init_grasp(self):
try:
p.removeBody(self.box_id)
except:
pass
pos_traj = np.load(os.path.join(self.env_root, 'init', 'pos.npy'))
orn_traj = np.load(os.path.join(self.env_root, 'init', 'orn.npy'))
self.fix_orn = np.load(os.path.join(self.env_root, 'init', 'orn.npy'))
for j in range(7):
self.p.resetJointState(self.robotId, j, self.data_q[0][j],
self.data_dq[0][j])
self.robot.gripperControl(0)
for init_t in range(100):
box = p.getAABB(self.obj_id, -1)
center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])]
center[0] -= 0.05
center[1] -= 0.05
center[2] += 0.03
# center = (box[0]+box[1])*0.5
points = np.array([pos_traj[0], center])
start_id = 0
init_traj = point2traj(points)
start_id = self.move(init_traj, orn_traj, start_id)
# grasping
grasp_stage_num = 10
for grasp_t in range(grasp_stage_num):
gripperPos = grasp_t / float(grasp_stage_num) * 180.0
self.robot.gripperControl(gripperPos)
start_id += 1
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([pos, [pos[0], pos[1] + 0.05, pos[2] + 0.3]])
start_id = self.move(up_traj, orn_traj, start_id)
if self.opt.rand_start == 'rand':
# move in z-axis direction
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([
pos, [pos[0], pos[1], pos[2] + (random.random() - 0.5) * 0.1]
])
start_id = self.move(up_traj, orn_traj, start_id)
# move in y-axis direction
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([
pos,
[pos[0], pos[1] + (random.random() - 0.5) * 0.2 + 0.2, pos[2]]
])
start_id = self.move(up_traj, orn_traj, start_id)
# move in x-axis direction
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([
pos, [pos[0] + (random.random() - 0.5) * 0.2, pos[1], pos[2]]
])
start_id = self.move(up_traj, orn_traj, start_id)
elif self.opt.rand_start == 'two':
prob = random.random()
if prob < 0.5:
pos = p.getLinkState(self.robotId, 7)[0]
up_traj = point2traj([pos, [pos[0], pos[1] + 0.2, pos[2]]])
start_id = self.move(up_traj, orn_traj, start_id)
if self.opt.rand_box == 'rand':
self.box_file = os.path.join(self.env_root,
"urdf/openbox/openbox.urdf")
self.box_position = [
0.42 + (random.random() - 0.5) * 0.2,
0.00 + (random.random() - 0.5) * 0.4, 0.34
]
self.box_scaling = 0.00037
self.box_orientation = p.getQuaternionFromEuler(
[0, 0, math.pi / 2])
self.box_id = p.loadURDF(fileName=self.box_file,
basePosition=self.box_position,
baseOrientation=self.box_orientation,
globalScaling=self.box_scaling
) #, physicsClientId=self.physical_id)
else:
self.box_file = os.path.join(self.env_root,
"urdf/openbox/openbox.urdf")
self.box_position = [0.42, 0.00, 0.34]
self.box_scaling = 0.00037
self.box_orientation = p.getQuaternionFromEuler(
[0, 0, math.pi / 2])
self.box_id = p.loadURDF(fileName=self.box_file,
basePosition=self.box_position,
baseOrientation=self.box_orientation,
globalScaling=self.box_scaling
) #, physicsClientId=self.physical_id)
texture_path = os.path.join(self.env_root, 'texture/sun_textures')
texture_file = os.path.join(
texture_path,
random.sample(os.listdir(texture_path), 1)[0])
textid = p.loadTexture(texture_file)
# p.changeVisualShape (self.box_id, -1, rgbaColor=[1, 1, 1, 0.9])
# p.changeVisualShape (self.box_id, -1, textureUniqueId=textid)
p.changeVisualShape(self.box_id, -1, rgbaColor=[1, 0, 0, 1])
self.start_pos = p.getLinkState(self.robotId, 7)[0]
box = p.getAABB(self.box_id, -1)
box_center = [(x + y) * 0.5 for x, y in zip(box[0], box[1])]
obj = p.getAABB(self.obj_id, -1)
obj_center = [(x + y) * 0.5 for x, y in zip(obj[0], obj[1])]
self.last_aabb_dist = sum([
(x - y)**2 for x, y in zip(box_center, obj_center)
])**0.5
POSITION = 1
ORIENTATION = 2
BUTTONS = 6
p.setRealTimeSimulation(1)
controllerId = -1
while True:
events = p.getVREvents()
for e in (events):
#print (e[BUTTONS])
if (e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED):
if (controllerId >= 0):
controllerId = -1
else:
controllerId = e[0]
if (e[0] == controllerId):
if (robot_cid >= 0):
p.changeConstraint(robot_cid, e[POSITION], e[ORIENTATION], maxForce=5000)
if (e[BUTTONS][32] & p.VR_BUTTON_WAS_TRIGGERED):
if (robot_cid >= 0):
p.removeConstraint(robot_cid)
#q = [0,0,0,1]
euler = p.getEulerFromQuaternion(e[ORIENTATION])
q = p.getQuaternionFromEuler([euler[0], euler[1], 0]) #-euler[0],-euler[1],-euler[2]])
robot_cid = p.createConstraint(minitaur, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0.1, 0, 0],
e[POSITION], q, e[ORIENTATION])
POSITION=1 ORIENTATION=2 BUTTONS=6 p.setRealTimeSimulation(1) controllerId = -1 while True: events = p.getVREvents() for e in (events): #print (e[BUTTONS]) if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED ): if (controllerId >= 0): controllerId = -1 else: controllerId = e[0] if (e[0] == controllerId): if (robot_cid>=0): p.changeConstraint(robot_cid,e[POSITION],e[ORIENTATION], maxForce=5000) if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED ): if (robot_cid>=0): p.removeConstraint(robot_cid) #q = [0,0,0,1] euler = p.getEulerFromQuaternion(e[ORIENTATION]) q = p.getQuaternionFromEuler([euler[0],euler[1],0]) #-euler[0],-euler[1],-euler[2]]) robot_cid = p.createConstraint(minitaur,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.1,0,0],e[POSITION],q,e[ORIENTATION])
for i in range(segmentLength):
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
segmentStart = segmentStart - 1
for i in range(segmentLength):
height = 0
if (i % 2):
height = 1
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1 + height])
segmentStart = segmentStart - 1
baseOrientation = p.getQuaternionFromEuler([math.pi / 2., 0, math.pi / 2.])
for i in range(segmentLength):
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, 0, -0.1])
segmentStart = segmentStart - 1
if (i % 2):
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
basePosition=[segmentStart, i % 3, -0.1 + height + boxHalfWidth],
baseOrientation=baseOrientation)
for i in range(segmentLength):
p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=colBoxId,
import pybullet as p
import pybullet_data
import time
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF
p.setGravity(0,0,-10)
planeId = p.loadURDF("road_plain.urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
p.stepSimulation()
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
print(cubePos,cubeOrn)
time.sleep(5)
p.disconnect()
import pybullet as p
from time import sleep
import matplotlib.pyplot as plt
import numpy as np
physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)
bearStartPos1 = [-3.3,0,0]
bearStartOrientation1 = p.getQuaternionFromEuler([0,0,0])
bearId1 = p.loadURDF("plane.urdf", bearStartPos1, bearStartOrientation1)
bearStartPos2 = [0,0,0]
bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0])
bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2)
textureId = p.loadTexture("checker_grid.jpg")
#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId)
#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId)
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
while 1:
if (useRealTimeSimulation):
camera = p.getDebugVisualizerCamera()
viewMat = camera[2]
projMat = camera[3]
#An example of setting the view matrix for the projective texture.
#viewMat = p.computeViewMatrix(cameraEyePosition=[7,0,0], cameraTargetPosition=[0,0,0], cameraUpVector=[0,0,1])
#coord = p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)
#coordPos = [0,0,0]
#coordOrnEuler = [0,0,0]
#coordPos= [0.7000000000000004, 0, 0.22000000000000006]
#coordOrnEuler= [0, -0.2617993877991496, 0]
coordPos = [0.07, 0, -0.6900000000000004]
coordOrnEuler = [0, 0, 0]
coordPos2 = [0, 0, 0]
coordOrnEuler2 = [0, 0, 0]
invPos, invOrn = p.invertTransform(coordPos, p.getQuaternionFromEuler(coordOrnEuler))
mPos, mOrn = p.multiplyTransforms(invPos, invOrn, coordPos2,
p.getQuaternionFromEuler(coordOrnEuler2))
eul = p.getEulerFromQuaternion(mOrn)
print("rpy=\"", eul[0], eul[1], eul[2], "\" xyz=\"", mPos[0], mPos[1], mPos[2])
shift = 0
gui = 1
frame = 0
states = []
states.append(p.saveState())
#observations=[]
#observations.append(obs)
running = True
