def constrain(obj1, obj2, link, cpos, pos, id_lookup, constraints, ur5_dist):
"""
Constrain two objects
:params:
obj1 - object to be constrained
obj2 - target object to which obj1 is constrained
link - link lookup for objects to be constrained
id_lookup - id dictionary to lookup object id by name
constraints - current list of constraints
ur5_dist - dictionary to lookup distance from ur5 gripper
:return:
cid - constraint id
"""
if obj1 in constraints.keys():
p.removeConstraint(constraints[obj1][1])
count = 0 # count checks where to place on target object
for obj in constraints.keys():
if constraints[obj][0] == obj2:
count += 1
print("New constraint=", obj1, " on ", obj2)
# parent is the target, child is the object
if obj2 == "ur5":
cid = p.createConstraint(id_lookup[obj2], link[obj2], id_lookup[obj1], link[obj1], p.JOINT_POINT2POINT, [0, 0, 0],
parentFramePosition=ur5_dist[obj1],
childFramePosition=cpos[obj1][0],
childFrameOrientation=[0,0,0,0])
else:
cid = p.createConstraint(id_lookup[obj2], link[obj2], id_lookup[obj1], link[obj1], p.JOINT_POINT2POINT, [0, 0, 0],
parentFramePosition=pos[obj2][count],
childFramePosition=cpos[obj1][0],
childFrameOrientation=[0,0,0,0])
return cid
def Reset(self, reload_urdf=True):
"""Reset the minitaur to its initial states.
Args:
reload_urdf: Whether to reload the urdf file. If not, Reset() just place
the minitaur back to its starting position.
"""
if reload_urdf:
if self._self_collision_enabled:
self.quadruped = pybullet.loadURDF(
"quadruped/minitaur.urdf",
INIT_POSITION,
flags=pybullet.URDF_USE_SELF_COLLISION)
else:
self.quadruped = pybullet.loadURDF("quadruped/minitaur.urdf",
INIT_POSITION)
self._BuildJointNameToIdDict()
self._BuildMotorIdList()
self._RecordMassInfoFromURDF()
self.ResetPose(add_constraint=True)
if self._on_rack:
pybullet.createConstraint(self.quadruped, -1, -1, -1,
pybullet.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], [0, 0, 1])
else:
pybullet.resetBasePositionAndOrientation(self.quadruped,
INIT_POSITION,
INIT_ORIENTATION)
pybullet.resetBaseVelocity(self.quadruped, [0, 0, 0], [0, 0, 0])
self.ResetPose(add_constraint=False)
self._overheat_counter = np.zeros(self.num_motors)
self._motor_enabled_list = [True] * self.num_motors
def __init__(self, robot, tool):
self.robot = robot
self.tool = tool
pos = [0.487, 0.109, 0.421]
rot = p.getQuaternionFromEuler([np.pi, 0, 0])
urdf = 'assets/ur5/gripper/robotiq_2f_85.urdf'
self.body = p.loadURDF(urdf, pos, rot)
self.n_joints = p.getNumJoints(self.body)
self.activated = False
# Connect gripper base to robot tool
p.createConstraint(self.robot,
tool,
self.body,
0,
jointType=p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, -0.05])
# Set friction coefficients for gripper fingers
for i in range(p.getNumJoints(self.body)):
p.changeDynamics(
self.body,
i,
lateralFriction=1.5,
spinningFriction=1.0,
rollingFriction=0.0001,
# rollingFriction=1.0,
frictionAnchor=True
) # contactStiffness=0.0, contactDamping=0.0
# Start thread to handle additional gripper constraints
self.motor_joint = 1
def main():
# Open GUI and set pybullet_data in the path
p.connect(p.GUI)
#p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Load plane contained in pybullet_data
p.loadURDF(os.path.join(pybullet_data.getDataPath(),"plane.urdf"))
# Set gravity for simulation
p.setGravity(0,0,-9.8)
# load robot model
robotIds = p.loadSDF(os.path.join(robot_data.getDataPath(), "iCub/icub_fixed_model.sdf"))
icubId = robotIds[0]
# set constraint between base_link and world
p.createConstraint(icubId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],
[p.getBasePositionAndOrientation(icubId)[0][0],
p.getBasePositionAndOrientation(icubId)[0][1],
p.getBasePositionAndOrientation(icubId)[0][2]*1.2],
p.getBasePositionAndOrientation(icubId)[1])
##init_pos for standing
# without FT_sensors
init_pos = [0]*15 + [-29.4, 28.8, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 44.59, 0, 0, 0]
# with FT_sensors
#init_pos = [0]*19 + [-29.4, 28.8, 0, 0, 44.59, 0, 0, 0, 0.47, 0, 0, -29.4, 30.4, 0, 0, 44.59, 0, 0, 0]
# all set to zero
#init_pos = [0]*p.getNumJoints(icubId)
# Load other objects
p.loadURDF(os.path.join(pybullet_data.getDataPath(),"table/table.urdf"), [1.1, 0.0, 0.0])
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "lego/lego.urdf"), [0.5,0.0,0.8])
# add debug slider
jointIds=[]
paramIds=[]
joints_num = p.getNumJoints(icubId)
print("len init_pos ",len(init_pos))
print("len num joints", joints_num)
for j in range (joints_num):
info = p.getJointInfo(icubId,j)
jointName = info[1]
#jointType = info[2]
jointIds.append(j)
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"), info[8], info[9], init_pos[j]/180*m.pi))
while True:
for i in range(joints_num):
p.setJointMotorControl2(icubId, i, p.POSITION_CONTROL,
targetPosition=p.readUserDebugParameter(i),
targetVelocity=0.0, positionGain=0.25, velocityGain=0.75, force=50)
p.stepSimulation()
time.sleep(0.01)
def __init__(self, n_joints, pos, radius, parent=None):
'''
Create a pybullet tentacle
'''
mass = 1
visualShapeId = -1
self.particles = [None] * n_joints
self.radius = radius
if parent is None:
base_shape = p.createCollisionShape(p.GEOM_PLANE)
self.base = p.createMultiBody(0, base_shape, -1, pos)
else:
self.base = parent
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=self.radius)
p.setCollisionFilterGroupMask(self.base,-1,0,0)
for i in range (n_joints):
self.particles[i] = p.createMultiBody(mass, colSphereId ,visualShapeId,
[pos[0], pos[1], pos[2] + i*2*self.radius + self.radius])
if i > 0:
p.createConstraint(self.particles[i-1], -1, self.particles[i], -1, p.JOINT_FIXED,[0,0,0], [0,0, 2*self.radius], [0,0,0])
else:
p.setCollisionFilterPair(self.base, self.particles[i], -1, -1, True)
p.createConstraint(self.base, -1, self.particles[i], -1, p.JOINT_FIXED, [0,0,0], [0,0,self.radius], [0,0,0], [0,0,0,1])
p.changeDynamics(self.particles[i],-1, mass=1, linearDamping=1)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
def attach(
self, new_robot, link_name, position=(0, 0, 0), orientation=(0, 0, 0, 1)
):
"""
Attach a new robot (`new_robot`) to self by creating a fixed joint between
a specific link (`link_name`) and the base of the new robot.
"""
assert isinstance(new_robot, Robot)
link_idx = self.get_joint_index_by_name(link_name)
link_pos = self.get_link_state(link_idx).link_world_position
link_pos = np.array(link_pos) + np.array(position)
new_robot.set_base_pose(link_pos)
p.createConstraint(
parentBodyUniqueId=self.id,
parentLinkIndex=link_idx,
childBodyUniqueId=new_robot.id,
childLinkIndex=-1,
jointType=p.JOINT_FIXED,
jointAxis=[0, 0, 0], # ignored for JOINT_FIXED
parentFramePosition=position,
childFramePosition=[0, 0, 0],
parentFrameOrientation=orientation,
childFrameOrientation=[0, 0, 0, 1],
**self._client_kwargs,
)
def addClosedChainConstraint(self, linkName, linkFrame2Joint):
linkState = p.getLinkState(self.cliffordID,
self.linkNameToID[linkName],
physicsClientId=self.physicsClientId)
bodyState = p.getBasePositionAndOrientation(
self.cliffordID, physicsClientId=self.physicsClientId)
world2joint = p.multiplyTransforms(linkState[4], linkState[5],
linkFrame2Joint, [0, 0, 0, 1])
body2world = p.invertTransform(bodyState[0], bodyState[1])
body2joint = p.multiplyTransforms(body2world[0], body2world[1],
world2joint[0], world2joint[1])
linkcm2world = p.invertTransform(linkState[0], linkState[1])
linkcm2joint = p.multiplyTransforms(linkcm2world[0], linkcm2world[1],
world2joint[0], world2joint[1])
c = p.createConstraint(self.cliffordID,
-1,
self.cliffordID,
self.linkNameToID[linkName],
p.JOINT_POINT2POINT, [0, 0, 0],
body2joint[0],
linkcm2joint[0],
physicsClientId=self.physicsClientId)
c = p.createConstraint(self.cliffordID,
self.jointNameToID['axle2frwheel'],
self.cliffordID,
self.jointNameToID['axle2flwheel'],
jointType=p.JOINT_GEAR,
jointAxis=[0, 1, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
physicsClientId=self.physicsClientId)
p.changeConstraint(c,
gearRatio=-1,
maxForce=10000,
physicsClientId=self.physicsClientId)
def __init__(self, config, parent=None):
Receptor.__init__(self)
self.name = config.name
self.position = config.get('xyz', [0., 0., 0.])
self.orientation = p.getQuaternionFromEuler(config.get('rpy', [0., 0., 0.]))
use_fixed_base = config.get('use_fixed_base', False)
scale = config.get('scale', 1.0)
urdf = config.get('model')
try:
full_urdf_path = next(
os.path.join(path, urdf) for path in urdf_path if os.path.isfile(os.path.join(path, urdf)))
except StopIteration:
raise ValueError('Could not find URDF: {urdf} in {urdf_path}')
flags = 0
if config.get('URDF_USE_MATERIAL_COLORS_FROM_MTL', False):
flags = flags | p.URDF_USE_MATERIAL_COLORS_FROM_MTL
if config.get('URDF_USE_SELF_COLLISION_EXCLUDE_PARENT', False):
flags = flags | p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT
flags = flags | p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
flags = flags | p.URDF_USE_SELF_COLLISION
if config.get('URDF_USE_IMPLICIT_CYLINDER', False):
flags |= p.URDF_USE_IMPLICIT_CYLINDER
if config.get('URDF_MERGE_FIXED_LINKS', False):
flags |= p.URDF_MERGE_FIXED_LINKS
self.uid = p.loadURDF(full_urdf_path, useFixedBase=use_fixed_base, globalScaling=scale, flags=flags)
if parent is None:
p.resetBasePositionAndOrientation(self.uid, self.position, self.orientation)
else:
parent_frame_id = parent.get_frame_id(config.get('parent_frame')) if 'parent_frame' in config else -1
child_frame_id = self.get_frame_id(config.get('child_frame')) if 'child_frame' in config else -1
pose = p.getLinkState(parent.uid, parent_frame_id)[:2] if parent_frame_id != -1 else p.getBasePositionAndOrientation(parent.uid)[:2]
p.resetBasePositionAndOrientation(self.uid, *pose)
p.createConstraint(parent.uid, parent_frame_id, self.uid, child_frame_id, p.JOINT_FIXED, [0, 0, 1],
self.position, [0, 0, 0], self.orientation, [0, 0, 0, 1])
if 'mass' in config:
p.changeDynamics(self.uid, -1, mass=config.get('mass'))
if 'color' in config:
p.changeVisualShape(self.uid, -1, rgbaColor=config.get('color'))
self.addons = OrderedDict(
sorted(
{child.name: AddonFactory.build(child.get('addon'), self, child)
for child in config.find_all('addon')}.items(),
key=lambda t: t[0]))
self.models = OrderedDict(
sorted({child.name: Model(child, self)
for child in config.find_all('model')}.items(), key=lambda t: t[0]))
def __init__(self, pos=None, orn=None, fixed_height=None):
# 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(TeststandConfig.paths["package"])
self.urdf_path = TeststandConfig.urdf_path
self.robotId = pybullet.loadURDF(
self.urdf_path,
pos,
orn,
flags=pybullet.URDF_USE_INERTIA_FROM_FILE,
useFixedBase=False,
)
self.pin_robot = TeststandConfig.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.base_link_name = "base_link"
controlled_joints = ["joint_z", "HFE", "KFE"]
self.joint_names = controlled_joints
# Creates the wrapper by calling the super.__init__.
super(TeststandRobot, self).__init__(self.robotId,
self.pin_robot,
controlled_joints, ["END"],
useFixedBase=True)
self.nb_dof = self.nv
if fixed_height:
pybullet.createConstraint(
self.robotId,
0,
-1,
-1,
pybullet.JOINT_FIXED,
[0, 0, 0],
[0, 0, 0.0],
[0, 0, fixed_height],
)
def __init__(self,cap=cv2.VideoCapture(0),mod="Direct",epsilon=150,preprocess=None,resolution=(56,56,3)):
self.cap = cap
if mod == "GUI":
self.clientId = p.connect(p.GUI)
## This is just to see the hand through opengl window so hence set this as you see the hand as you want to see
else:
self.clientId = p.connect(p.DIRECT)
#p.setRealTimeSimulation(1,physicsClientId=self.clientId)
p.resetDebugVisualizerCamera(cameraDistance=2, cameraYaw=0, cameraPitch=-40, cameraTargetPosition=[0,0,2],physicsClientId=self.clientId)
self.action_space = spaces.Box(low=np.array([0]*10+[-0.52,-1.04]) ,high=np.array([1.55]*10+[0.52,1.04]))
## down and up (thumb, index, middle, ring, little) , wrist, elbow
if len(resolution)!=3:
raise Exception("Only a ndim n=3 image can be given as a input")
self.res=resolution
self.observation_space = spaces.Box(0,2.55,shape=(self.res[0],self.res[1]*2,self.res[2]))
p.setAdditionalSearchPath(pybullet_data.getDataPath())
self.plane = p.loadURDF( "plane.urdf" , physicsClientId=self.clientId)
p.setAdditionalSearchPath(os.path.abspath("Simulation"))
self.handid = p.loadURDF(currentdir+"/hand.urdf",physicsClientId=self.clientId)
for i in range(p.getNumJoints(self.handid,physicsClientId=self.clientId)):
print(p.getJointInfo(bodyUniqueId=self.handid,jointIndex=i,physicsClientId=self.clientId))
if preprocess is None:
self.hand_thresh=self.handmask
else:
self.hand_thresh=preprocess
self.epsilon=epsilon ## Find a good one and set as default
self.seed(int(time.time()))
## THis is to match up the no of pixels of our PHATTTT
p.createConstraint(
self.handid,
-1,
-1,
-1,
p.JOINT_FIXED,
(0, 0, 1),
(0, 0, 0),
(0, 0, 0),
physicsClientId=self.clientId
)
finger_joint_indices = (
(2, 3), # thumb
(4, 5), # index
(6, 7), # middle
(8, 9), # ring
(10, 11), # little
)
self.hand = robo_hand(self.handid,finger_joint_indices,clientId=self.clientId)
self.resetState = p.saveState()
self.reset()
def create(self):
p.connect(p.GUI)
p.createMultiBody(0, 0)
p.setGravity(0, 0, -9.8)
p.setAdditionalSearchPath(data.getDataPath())
p.loadSDF("stadium.sdf")
#p.setRealTimeSimulation(True)
#bulid a plate
self.plateId = p.createCollisionShape(
p.GEOM_BOX,
halfExtents=[self.plateLong, self.plateWidth, self.plateHigh])
#bulid a motor
self.motorId = p.createCollisionShape(p.GEOM_CYLINDER,
radius=self.motorRadius,
height=self.motorHeight)
#bulid a ball
self.ballId = p.createCollisionShape(p.GEOM_SPHERE,
radius=self.ballRadius,
height=self.motorHeight)
self.ballUid = p.createMultiBody(self.ballMass, self.ballId,
self.visualShapeId, self.ballStartPos,
self.ballStartOrn)
#bulit a multi-body
self.linkCollisionShapeIndices = [self.motorId]
self.controlerUid = p.createMultiBody(
self.mass,
self.plateId,
self.visualShapeId,
self.basePosition,
self.baseOrientation,
linkMasses=self.link_Masses,
linkCollisionShapeIndices=self.linkCollisionShapeIndices,
linkVisualShapeIndices=self.linkVisualShapeIndices,
linkPositions=self.linkPositions,
linkOrientations=self.linkOrientations,
linkInertialFramePositions=self.linkInertialFramePositions,
linkInertialFrameOrientations=self.linkInertialFrameOrientations,
linkParentIndices=self.indices,
linkJointTypes=self.jointTypes,
linkJointAxis=self.axis)
#change some physical parameters
p.changeDynamics(self.controlerUid,
-1,
spinningFriction=0.01,
rollingFriction=0.01,
linearDamping=0.0)
#constraint objects
#p.createConstraint(self.motorId,-1,-1,-1,p.JOINT_POINT2POINT,[0,0,0],[0,0,0],[0,0,1.5])
p.createConstraint(self.controlerUid, -1, -1, -1, p.JOINT_POINT2POINT,
[0, 0, 0], [0, 0, 0], [0, 0, 1.5])
p.setRealTimeSimulation(self.useRealTimeSim)
def __init__(self,
sensor_name,
robot_id=None,
robot_link=None,
position_offset=[0, 0, 0],
orientation_offset=[0, 0, 0, 1],
fixed_pose=True,
urdf_file=None,
urdf_flags=0):
'''
Args:
sensor_name (str): Identifier for this sensor
Optional Args:
robot_id (int): PyBullet id of robot this belongs to
robot_link (int): Link id where this joint is 'mounted'
position_offset (Tuple of 3 floats): X,Y,Z position of sensor relative to mount point (or world frame if robot_id is None)
orientation_offset (Tuple of 4 floats): Quaternion orientation of sensor relative to mount point (or world frame if robot_id is None)
fixed_pose (Bool): True if fixed in place, False if movable/subject to physics
urdf_file (str): Location of the sensor's URDF file to render
urdf_flags (int): PyBullet URDF flags to load with the model
'''
self._name = sensor_name
self._simulator_id = None
self._robot_id = robot_id
self._robot_link = robot_link
self._position_offset = np.array(
position_offset
) # Initial position, could change over the course of simulation!
self._orientation_offset = np.array(
orientation_offset
) # Initial orientation, could change over the course of simulation!
self._debug_mode = False # Set to True to trigger a Sensor's debugging mode (e.g., rendering lasers, line of sight, etc.)
# If there's a physical model to render to represent this sensor, we have to care about physics
if urdf_file is not None:
self._simulator_id = p.loadURDF(urdf_file,
basePosition=position_offset,
baseOrientation=orientation_offset,
useFixedBase=fixed_pose,
flags=urdf_flags)
if self._robot_id is None:
# Sensor exists independent of a robot
p.createConstraint(self._simulator_id, -1, -1, -1,
p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
position_offset, orientation_offset)
else:
# Sensor is part of the robot, attach it to robot_id on its robot_link
p.createConstraint(self._simulator_id, -1, self._robot_id,
self._robot_link, p.JOINT_FIXED, [0, 0, 0],
[0, 0, 0], position_offset,
orientation_offset)
def place(self, pos, rot, joints):
pb.resetBasePositionAndOrientation(self.handle, pos, rot)
pb.createConstraint(self.handle, -1, -1, -1, pb.JOINT_FIXED, pos,
[0, 0, 0], rot)
for i, q in enumerate(joints):
pb.resetJointState(self.handle, i, q)
# gripper state
for joint in self.gripper_indices:
pb.resetJointState(self.handle, joint, 0.)
# send commands
self.arm(joints, )
self.gripper(self.gripperOpenCommand())
def setup(self, dims=None):
#create constraint and a second cup
self.cupStartPos = (0, -0.6, 0)
self.cupStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
#pick random cup
if dims is None:
self.cup_name = np.random.choice(self.cup_to_dims.keys())
else:
found_cup = False
for name in self.cup_to_dims.keys():
if self.cup_to_dims[name][0] == dims[0] and self.cup_to_dims[
name][1] == dims[1]:
found_cup = True
self.cup_name = name
assert (found_cup)
cup_file = "urdf/cup/" + self.cup_name
self.target_cup = p.loadURDF(cup_file,
self.cupStartPos,
self.cupStartOrientation,
globalScaling=k * 5)
self.base_world.drop_beads_in_cup()
self.cid = p.createConstraint(self.base_world.cupID, -1, -1, -1,
p.JOINT_FIXED, self.cupStartPos,
self.cupStartOrientation, [0, 0, 1])
self.bullet_id = p.saveState()
def attach(self, object, parent_link=None, child_link=None):
"""
This method attaches two objects. This will be done by creating a virtual fixed joint between the two objects.
After the attachment the attachment event of the BulletWorld will be fired.
It can only exist one attachment between two objects.
:param object: The object which should be attached to this object
:param parent_link: The Name of the link of this object to which the other object should be attached or -1
for the base position
:param child_link: The link Name of the other object to which this object should be attached or -1 for the base
"""
if object in self.attachments:
return
parent_link_id = -1 if parent_link is None else self.links[parent_link]
child_link_id = -1 if child_link is None else object.links[child_link]
world_gripper = p.getLinkState(
self.id, parent_link_id
)[4] if parent_link_id != -1 else self.get_position()
world_object = object.get_position()
gripper_object = p.multiplyTransforms(
p.invertTransform(world_gripper, [0, 0, 0, 1])[0], [0, 0, 0, 1],
world_object, [0, 0, 0, 1], self.world.client_id)[0]
cid = p.createConstraint(self.id,
parent_link_id,
object.id,
child_link_id,
p.JOINT_FIXED, [0, 1, 0],
gripper_object, [0, 0, 0],
physicsClientId=self.world.client_id)
p.changeConstraint(cid, maxForce=30)
self.attachments[object] = cid, parent_link_id
object.attachments[self] = cid, parent_link_id
self.world.attachment_event(self, [self, object])
def activateNailgun(nailGunID,
object1ID,
parentRefCOM,
childRefCOM,
childFrameOrn=[0, 0, 0]):
#check to see if they're touching
gunContact = p.getContactPoints(nailGunID, object1ID)
if not len(gunContact):
print('No Contact')
return False
#get rid of the nasty contact points-just use COM of nailgun + some z component to get pos, and just use straight up as orn
#use ray tracing to determine locations of constrain
gunPos, _ = p.getBasePositionAndOrientation(nailGunID)
rayPos = np.array(gunPos) + np.array([0, 0, 0.165])
rayOrn = np.array([0, 0, 1])
rayDist = 0.25
#rayOrn = np.array([0,0,1])
#Check and see if the ray intersects both objects
rayTest = p.rayTest(rayPos, rayPos + rayDist * rayOrn)[0]
hitID = rayTest[0]
hitPos = np.array(rayTest[3])
#Add constrain
constraintID = p.createConstraint(object1ID,
-1,
hitID,
-1,
p.JOINT_FIXED,
hitPos,
parentRefCOM,
childRefCOM,
childFrameOrientation=childFrameOrn)
print("Finished")
print(p.getConstraintInfo(constraintID))
return constraintID
def _set_attached_objects(self, prev_object):
"""
This method updates the positions of all attached objects. This is done
by calculating the new pose in world coordinate frame and setting the
base pose of the attached objects to this new pose.
After this the _set_attached_objects method of all attached objects
will be called.
:param prev_object: The object that called this method, this will be
excluded to prevent recursion in the update.
"""
for obj in self.attachments:
if obj in prev_object:
continue
if self.attachments[obj][2]:
# Updates the attachment transformation and contraint if the
# attachment is loos, instead of updating the position of all attached objects
link_T_object = self._calculate_transform(obj, self.attachments[obj][1])
link_id = self.get_link_id(self.attachments[obj][1]) if self.attachments[obj][1] else -1
self.attachments[obj][0] = link_T_object
obj.attachments[self][0] = p.invertTransform(link_T_object[0], link_T_object[1])
p.removeConstraint(self.cids[obj])
cid = p.createConstraint(self.id, link_id, obj.id, -1, p.JOINT_FIXED, [0, 0, 0], link_T_object[0], [0, 0, 0], link_T_object[1])
self.cids[obj] = cid
obj.cids[self] = cid
else:
# Updates the position of all attached objects
link_T_object = self.attachments[obj][0]
link_name = self.attachments[obj][1]
world_T_link = self.get_link_position_and_orientation(link_name) if link_name else self.get_position_and_orientation()
world_T_object = p.multiplyTransforms(world_T_link[0], world_T_link[1], link_T_object[0], link_T_object[1])
p.resetBasePositionAndOrientation(obj.id, world_T_object[0], world_T_object[1])
obj._set_attached_objects(prev_object + [self])
def add_fixed_constraint(body, robot, robot_link, max_force=None):
body_link = BASE_LINK
body_pose = body.get_pose()
#body_pose = get_com_pose(body, link=body_link)
#end_effector_pose = get_link_pose(robot, robot_link)
end_effector_pose = robot.get_com_pose(robot_link)
grasp_pose = pb_robot.geometry.multiply(pb_robot.geometry.invert(end_effector_pose), body_pose)
point, quat = grasp_pose
# TODO: can I do this when I'm not adjacent?
# joint axis in local frame (ignored for JOINT_FIXED)
#return p.createConstraint(robot, robot_link, body, body_link,
# p.JOINT_FIXED, jointAxis=unit_point(),
# parentFramePosition=unit_point(),
# childFramePosition=point,
# parentFrameOrientation=unit_quat(),
# childFrameOrientation=quat)
constraint = p.createConstraint(robot, robot_link, body, body_link, # Both seem to work
p.JOINT_FIXED, jointAxis=pb_robot.geometry.unit_point(),
parentFramePosition=point,
childFramePosition=pb_robot.geometry.unit_point(),
parentFrameOrientation=quat,
childFrameOrientation=pb_robot.geometry.unit_quat(),
physicsClientId=CLIENT)
if max_force is not None:
p.changeConstraint(constraint, maxForce=max_force, physicsClientId=CLIENT)
return constraint
def reset(self):
objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
self.kukaUid = objects[0]
p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
self.numJoints = p.getNumJoints(self.kukaUid)
for jointIndex in range (self.numJoints):
p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
#p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,0,0)
self.endEffectorPos = [0.537,0.0,0.5]
self.endEffectorAngle = 0
self.motorNames = []
self.motorIndices = []
import random
xpos = 0.55 +0.12*random.random()
ypos = 0 +0.2*random.random()
ang = 3.14*0.5+3.1415925438*random.random()
orn = p.getQuaternionFromEuler([0,0,ang])
orn = np.array((-0.0, 1, 0.0, 0.0))
pos = (0.5321085918022986, -0.0011177175302174629, 0.2522914797947443),
self.constraint = p.createConstraint(self.kukaUid,7,-1,-1,p.JOINT_FIXED,pos,orn,[0.500000,0.300006,0.700000])
def load_agent(self):
obj = pb.getBasePositionAndOrientation(self.obj_to_classify)
target = obj[0]
xyz = [target[0] - 1.25, target[1], target[2]]
#xyz=[0,-1.25,0]
orientation = [0, 1, 0, 1]
self.agent = pb.createCollisionShape(
pb.GEOM_BOX,
halfExtents=[self.wandSide, self.wandSide, self.wandLength])
self.agent_mb = pb.createMultiBody(1,
self.agent,
-1,
basePosition=xyz,
baseOrientation=orientation)
pb.changeVisualShape(self.agent_mb, -1, rgbaColor=[1, 1, 0, 1])
self.agent_cid = pb.createConstraint(self.agent_mb,
-1,
-1,
-1,
pb.JOINT_FIXED, [0, 0, 0],
[0, 0, 0],
xyz,
childFrameOrientation=orientation)
self.fov = 90
def __init__(self, server, **kwargs):
super().__init__(server, **kwargs)
# Add the constraints
parents = ["top_11", "top_12", "top_13"]
children = {
"top_11": ["ITF_31"],
"top_12": ["ITF_22", "ITF_12"],
"top_13": ["ITF_23", "ITF_33"],
}
self.constraints = {}
for parent in parents:
parent_id = self.joints[parent]["jointIndex"]
for child in children[parent]:
constraint_name = parent + "_2_" + child
child_id = self.joints[child]["jointIndex"]
# Create a p2p connection
constraint_id = pybullet.createConstraint(
parentBodyUniqueId=self.id,
parentLinkIndex=parent_id,
childBodyUniqueId=self.id,
childLinkIndex=child_id,
jointType=pybullet.JOINT_POINT2POINT,
jointAxis=(0, 0, 0),
parentFramePosition=(0, 0, 0),
childFramePosition=(0, 0, 0),
)
# Store the constraint information
self.constraints.update({constraint_name: constraint_id})
def attach(self, object, link=None, loose=False):
"""
This method attaches an other object to this object. This is done by
saving the transformation between the given link, if there is one, and
the base pose of the other object. Additional the name of the link, to
which the obejct is attached, will be saved.
Furthermore, a constraint of pybullet will be created so the attachment
also works in the simulation.
:param object: The other object that should be attached
:param link: The link of this obejct to which the other object should be
attached.
"""
link_id = self.get_link_id(link) if link else -1
link_T_object = self._calculate_transform(object, link)
self.attachments[object] = [link_T_object, link, loose]
object.attachments[self] = [
p.invertTransform(link_T_object[0], link_T_object[1]), None, False
]
cid = p.createConstraint(self.id,
link_id,
object.id,
-1,
p.JOINT_FIXED, [0, 1, 0],
link_T_object[0], [0, 0, 0],
link_T_object[1],
physicsClientId=self.world.client_id)
self.cids[object] = cid
object.cids[self] = cid
self.world.attachment_event(self, [self, object])
def robot_specific_reset(self):
WalkerBase.robot_specific_reset(self)
humanoidId = -1
numBodies = p.getNumBodies()
for i in range (numBodies):
bodyInfo = p.getBodyInfo(i)
if bodyInfo[1].decode("ascii") == 'humanoid':
humanoidId = i
## Spherical radiance/glass shield to protect the robot's camera
if self.glass_id is None:
glass_id = p.loadMJCF(os.path.join(self.physics_model_dir, "glass.xml"))[0]
#print("setting up glass", glass_id, humanoidId)
p.changeVisualShape(glass_id, -1, rgbaColor=[0, 0, 0, 0])
cid = p.createConstraint(humanoidId, -1, glass_id,-1,p.JOINT_FIXED,[0,0,0],[0,0,1.4],[0,0,1])
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
def reset(self):
self.panda_id = p.loadURDF(self.urdf_root_path,
basePosition=self.base_position,
useFixedBase=True)
for i in range(self.motor_count):
p.resetJointState(self.panda_id, i, self.start_joint_positions[i])
p.setJointMotorControl2(
self.panda_id,
i,
p.POSITION_CONTROL,
targetPosition=self.start_joint_positions[i],
force=self.max_force)
state = p.getLinkState(self.panda_id, self.gripper_index)
for j in range(p.getNumJoints(self.panda_id)):
p.changeDynamics(self.panda_id,
j,
linearDamping=0,
angularDamping=0)
c = p.createConstraint(self.panda_id,
9,
self.panda_id,
10,
jointType=p.JOINT_GEAR,
jointAxis=[1, 0, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, erp=0.1, maxForce=50)
self.gripper_pos = list(state[0])
self.gripper_orn = list(p.getEulerFromQuaternion(list(state[1])))
def create_constraint(self,
parent_link,
child,
child_link,
joint_type=p.JOINT_FIXED,
joint_axis=[0, 0, 0],
parent_pos=[0, 0, 0],
child_pos=[0, 0, 0],
parent_orient=[0, 0, 0],
child_orient=[0, 0, 0]):
if len(parent_orient) < 4:
parent_orient = self.get_quaternion(parent_orient)
if len(child_orient) < 4:
child_orient = self.get_quaternion(child_orient)
return p.createConstraint(self.body,
parent_link,
child.body,
child_link,
joint_type,
joint_axis,
parent_pos,
child_pos,
parent_orient,
child_orient,
physicsClientId=self.id)
def __init__(self, robot, ee, obj_ids): # pylint: disable=unused-argument
"""Creates spatula and 'attaches' it to the robot."""
super().__init__()
# Load spatula model.
pose = ((0.487, 0.109, 0.438), p.getQuaternionFromEuler((np.pi, 0, 0)))
base = p.loadURDF('assets/ur5/spatula/spatula-base.urdf', pose[0], pose[1])
p.createConstraint(
parentBodyUniqueId=robot,
parentLinkIndex=ee,
childBodyUniqueId=base,
childLinkIndex=-1,
jointType=p.JOINT_FIXED,
jointAxis=(0, 0, 0),
parentFramePosition=(0, 0, 0),
childFramePosition=(0, 0, 0.01))
def activate(self):
"""Simulate suction using a rigid fixed constraint to contacted object."""
# TODO(andyzeng): check deformables logic.
# del def_ids
if not self.activated:
points = p.getContactPoints(bodyA=self.body, linkIndexA=0)
# print(points)
if points:
# Handle contact between suction with a rigid object.
for point in points:
obj_id, contact_link = point[2], point[4]
if obj_id in self.obj_ids['rigid']:
body_pose = p.getLinkState(self.body, 0)
obj_pose = p.getBasePositionAndOrientation(obj_id)
world_to_body = p.invertTransform(body_pose[0], body_pose[1])
obj_to_body = p.multiplyTransforms(world_to_body[0],
world_to_body[1],
obj_pose[0], obj_pose[1])
self.contact_constraint = p.createConstraint(
parentBodyUniqueId=self.body,
parentLinkIndex=0,
childBodyUniqueId=obj_id,
childLinkIndex=contact_link,
jointType=p.JOINT_FIXED,
jointAxis=(0, 0, 0),
parentFramePosition=obj_to_body[0],
parentFrameOrientation=obj_to_body[1],
childFramePosition=(0, 0, 0),
childFrameOrientation=(0, 0, 0))
self.activated = True
def load_agent(self):
xyz = [-1, -1, 0]
orientation = [0, 1, 0, 1]
self.agent = pb.createCollisionShape(
pb.GEOM_BOX,
halfExtents=[self.wandSide, self.wandSide, self.wandLength])
self.agent_mb = pb.createMultiBody(1,
self.agent,
-1,
basePosition=xyz,
baseOrientation=orientation)
pb.changeVisualShape(self.agent, -1, rgbaColor=[1, 1, 0, 1])
self.agent_cid = pb.createConstraint(self.agent,
-1,
-1,
-1,
pb.JOINT_FIXED, [0, 0, 0],
[0, 0, 0],
xyz,
childFrameOrientation=orientation)
self.fov = 45
def create_constraint_local(self,
parentBody,
childBody,
jointType='fixed',
parentLink=None,
childLink=None,
jointAxis=[1, 0, 0],
parentJointPosition=[0, 0, 0],
childJointPosition=[0, 0, 0],
parentJointOrientation=[0, 0, 0, 1],
childJointOrientation=[0, 0, 0, 1],
name_override=None):
if name_override is None:
counter = 0
constraintName = 'constraint_{}'.format(jointType.lower())
constraintId = constraintName
while constraintId in self.bodies:
constraintId = '{}.{}'.format(bodyName, counter)
counter += 1
else:
if name_override in self.constraints:
raise Exception('Constraint Id "{}" is already taken.'.format(
name_override))
constraintId = name_override
parent_bid = parentBody.bId()
parent_link = parentBody.get_link_index(
parentLink) if parentLink is not None else -1
child_bid = childBody.bId() if childBody is not None else -1
child_link = childBody.get_link_index(
childLink) if childLink is not None else -1
if jointType not in self.__joint_types:
raise Exception(
'Unknown joint type "{}". Supported types are: {}'.format(
jointType, ', '.join(self.__joint_types.keys())))
type = self.__joint_types[jointType]
bulletId = pb.createConstraint(parent_bid,
parent_link,
child_bid,
child_link,
type,
axis,
parentJointPosition,
childJointPosition,
parentJointOrientation,
childJointOrientation,
physicsClientId=self.__client_id)
constraint = Constraint(self, bulletId, jointType, parentBody,
childBody, parentJointPosition,
parentJointOrientation, childJointPosition,
childJointOrientation, jointAxis, parentLink,
childLink)
self.constraints[constraintId] = constraint
self.__cId_IdMap[bulletId] = constraintId
return constraint
def _setup_experiment(self):
# add plane to push on (slightly below the base of the robot)
self.planeId = p.loadURDF("plane.urdf", [0, 0, 0], useFixedBase=True)
self.pusherId = p.loadURDF(os.path.join(cfg.ROOT_DIR, "pusher.urdf"),
self.initPusherPos)
self.blockId = p.loadURDF(
os.path.join(cfg.ROOT_DIR, "block_big.urdf"), self.initBlockPos,
p.getQuaternionFromEuler([0, 0, self.initBlockYaw]))
# adjust dynamics for better stability
p.changeDynamics(self.planeId,
-1,
lateralFriction=0.3,
spinningFriction=0.0,
rollingFriction=0.0)
self.walls = []
wall_z = 0.05
if self.level == 0:
pass
elif self.level in [1, 4, 5]:
self.walls.append(
p.loadURDF(os.path.join(cfg.ROOT_DIR, "wall.urdf"),
[-0.55, -0.25, wall_z],
p.getQuaternionFromEuler([0, 0, 0]),
useFixedBase=True,
globalScaling=0.8))
elif self.level == 2:
translation = 10
self.walls.append(
p.loadURDF(os.path.join(cfg.ROOT_DIR, "wall.urdf"),
[-0.55 + translation, -0.25 + translation, wall_z],
p.getQuaternionFromEuler([0, 0, 0]),
useFixedBase=True,
globalScaling=0.8))
elif self.level in [3, 6]:
self.walls.append(
p.loadURDF(os.path.join(cfg.ROOT_DIR, "wall.urdf"),
[-0.3, 0.25, wall_z],
p.getQuaternionFromEuler([0, 0, -math.pi / 4]),
useFixedBase=True,
globalScaling=0.8))
if self.level == 2:
self.set_camera_position([10, 10])
else:
self.set_camera_position([0, 0])
self._draw_goal()
self.state = self._obs()
self._draw_state()
# set gravity
p.setGravity(0, 0, -10)
# set robot init config
self.pusherConstraint = p.createConstraint(self.pusherId, -1, -1, -1,
p.JOINT_FIXED, [0, 0, 1],
[0, 0, 0],
self.initPusherPos)
def resetPose(self):
kneeFrictionForce = 0
halfpi = 1.57079632679
kneeangle = -2.1834 #halfpi - acos(upper_leg_length / lower_leg_length)
#left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],self.motorDir[0]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],self.motorDir[0]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],self.motorDir[1]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.motorDir[1]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_leftL_joint', self.motorDir[0]*halfpi)
self.setMotorAngleByName('motor_front_leftR_joint', self.motorDir[1]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],self.motorDir[2]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],self.motorDir[2]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],self.motorDir[3]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.motorDir[3]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_leftL_joint',self.motorDir[2]*halfpi)
self.setMotorAngleByName('motor_back_leftR_joint',self.motorDir[3]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_leftR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],self.motorDir[4]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],self.motorDir[4]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],self.motorDir[5]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.motorDir[5]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_front_rightL_joint',self.motorDir[4]*halfpi)
self.setMotorAngleByName('motor_front_rightR_joint',self.motorDir[5]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_front_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],self.motorDir[6]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],self.motorDir[6]*kneeangle)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],self.motorDir[7]*halfpi)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.motorDir[7]*kneeangle)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
self.setMotorAngleByName('motor_back_rightL_joint',self.motorDir[6]*halfpi)
self.setMotorAngleByName('motor_back_rightR_joint',self.motorDir[7]*halfpi)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightL_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
p.setJointMotorControl2(bodyIndex=self.quadruped,jointIndex=self.jointNameToId['knee_back_rightR_link'],controlMode=p.VELOCITY_CONTROL,targetVelocity=0,force=kneeFrictionForce)
def resetPose(self):
#right front leg
self.disableAllMotors()
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
#left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
self.setMotorAngleByName('motor_front_leftR_joint', 1.57)
self.setMotorAngleByName('motor_front_leftL_joint',-1.57)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_back_rightR_joint', 1.57)
self.setMotorAngleByName('motor_back_rightL_joint',-1.57)
#left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
self.setMotorAngleByName('motor_back_leftR_joint', 1.57)
self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
import pybullet as p
import time
import math
p.connect(p.SHARED_MEMORY)
p.loadURDF("plane.urdf")
p.setGravity(0, 0, -1)
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/quadruped.urdf", 10, -2, 2)
# p.getNumJoints(1)
# right front leg
p.resetJointState(quadruped, 0, 1.57)
p.resetJointState(quadruped, 2, -2.2)
p.resetJointState(quadruped, 3, -1.57)
p.resetJointState(quadruped, 5, 2.2)
p.createConstraint(quadruped, 2, quadruped, 5, p.JOINT_POINT2POINT, [0, 0, 0], [0, 0.01, 0.2], [0, -0.015, 0.2])
p.setJointMotorControl(quadruped, 0, p.POSITION_CONTROL, 1.57, 1)
p.setJointMotorControl(quadruped, 1, p.VELOCITY_CONTROL, 0, 0)
p.setJointMotorControl(quadruped, 2, p.VELOCITY_CONTROL, 0, 0)
p.setJointMotorControl(quadruped, 3, p.POSITION_CONTROL, -1.57, 1)
p.setJointMotorControl(quadruped, 4, p.VELOCITY_CONTROL, 0, 0)
p.setJointMotorControl(quadruped, 5, p.VELOCITY_CONTROL, 0, 0)
# left front leg
p.resetJointState(quadruped, 6, 1.57)
p.resetJointState(quadruped, 8, -2.2)
p.resetJointState(quadruped, 9, -1.57)
p.resetJointState(quadruped, 11, 2.2)
p.createConstraint(quadruped, 8, quadruped, 11, p.JOINT_POINT2POINT, [0, 0, 0], [0, -0.01, 0.2], [0, 0.015, 0.2])
import pybullet as p
import time
import math
p.connect(p.SHARED_MEMORY)
p.loadURDF("plane.urdf")
quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
#p.getNumJoints(1)
#right front leg
p.resetJointState(quadruped,0,1.57)
p.resetJointState(quadruped,2,-2.2)
p.resetJointState(quadruped,3,-1.57)
p.resetJointState(quadruped,5,2.2)
p.createConstraint(quadruped,2,quadruped,5,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,1,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,2,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,4,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,5,p.VELOCITY_CONTROL,0,0)
#left front leg
p.resetJointState(quadruped,6,1.57)
p.resetJointState(quadruped,8,-2.2)
p.resetJointState(quadruped,9,-1.57)
p.resetJointState(quadruped,11,2.2)
p.createConstraint(quadruped,8,quadruped,11,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,1.57,1)
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
ORIENTATION=2
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)
p.setRealTimeSimulation(useRealTimeSim) # either this
p.loadURDF("plane.urdf")
#p.loadSDF("stadium.sdf")
car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
for i in range (p.getNumJoints(car)):
print (p.getJointInfo(car,i))
for wheel in range(p.getNumJoints(car)):
p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.getJointInfo(car,wheel)
wheels = [8,15]
print("----------------")
#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)
c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)
p.loadURDF("quadruped/minitaur.urdf", [-0.000046, -0.000068, 0.200774],
[-0.000701, 0.000387, -0.000252, 1.000000],
useFixedBase=False)
]
ob = objects[0]
jointPositions = [
0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000,
-2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193,
0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517
]
for ji in range(p.getNumJoints(ob)):
p.resetJointState(ob, ji, jointPositions[ji])
p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)
cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid0, maxForce=500.000000)
cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid1, maxForce=500.000000)
cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
[0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid2, maxForce=500.000000)
cid3 = p.createConstraint(1, 22, 1, 25, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
[0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
[0.000000, 0.000000, 0.000000, 1.000000],
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])
p.resetSimulation()
#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)]
#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=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(pr2_gripper,jointIndex,p.POSITION_CONTROL,targetPosition=0,force=0)
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
pr2_cid2 = p.createConstraint(pr2_gripper,0,pr2_gripper,2,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(pr2_cid2,gearRatio=1, erp=0.5, relativePositionTarget=0.5, maxForce=3)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
p.connect(p.GUI)
p.resetSimulation()
#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)]
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=")
print (pr2_gripper)
jointPositions=[ 0.550569, 0.000000, 0.549657, 0.000000 ]
for jointIndex in range (p.getNumJoints(pr2_gripper)):
p.resetJointState(pr2_gripper,jointIndex,jointPositions[jointIndex])
pr2_cid = p.createConstraint(pr2_gripper,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0.2,0,0],[0.500000,0.300006,0.700000])
print ("pr2_cid")
print (pr2_cid)
objects = [p.loadURDF("kuka_iiwa/model_vr_limits.urdf", 1.400000,-0.200000,0.600000,0.000000,0.000000,0.000000,1.000000)]
kuka = objects[0]
jointPositions=[ -0.000000, -0.000000, 0.000000, 1.570793, 0.000000, -1.036725, 0.000001 ]
for jointIndex in range (p.getNumJoints(kuka)):
p.resetJointState(kuka,jointIndex,jointPositions[jointIndex])
p.setJointMotorControl2(kuka,jointIndex,p.POSITION_CONTROL,jointPositions[jointIndex],0)
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.700000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.800000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("lego/lego.urdf", 1.000000,-0.200000,0.900000,0.000000,0.000000,0.000000,1.000000)]
objects = p.loadSDF("gripper/wsg50_one_motor_gripper_new_free_base.sdf")
kuka_gripper = objects[0]
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]]
p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0])
p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0])
p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0])
p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0])
humanoid = p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid2 = p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid3 = p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid4 = p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1])
humanoid2_fix = p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1])
humanoid3_fix = p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1])
humanoid3_fix = p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1])
startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447,
1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153,
0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495,
0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348]
startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-0.1050802848,0,0.935706195,0.08277326387,0.3002461862,0,0,0,0,0,1.114409628,0.3618553952,
-0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0,
-4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973]
p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1])
motC_lb_Id= p.addUserDebugParameter("motC_lb",-limitVal,limitVal,legposSleft)
erp_lb_Id= p.addUserDebugParameter("erp_lb",0,1,defaultERP)
relPosTarget_lb_Id= p.addUserDebugParameter("relPosTarget_lb",-limitVal,limitVal,0)
camTargetPos=[0.25,0.62,-0.15]
camDist = 2
camYaw = -2
camPitch=-16
p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)
c_rf = p.createConstraint(vision,knee_front_rightR_joint,vision,motor_front_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,maxForce=maxGearForce)
c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce)
c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce)
c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,maxForce=maxGearForce)
p.setRealTimeSimulation(1)
