def release(self):
"""
If suction off, detect contact between gripper and objects.
If suction on, detect contact between picked object and other objects.
"""
if self.activated:
self.activated = False
# Release gripped rigid object (if any).
if self.contact_constraint is not None:
try:
p.removeConstraint(self.contact_constraint)
self.contact_constraint = None
except:
pass
self.init_grip_distance = None
self.init_grip_item = None
# Release gripped deformable object (if any).
if self.def_grip_anchors is not None:
for anchorId in self.def_grip_anchors:
p.removeConstraint(anchorId)
self.def_grip_anchors = None
self.def_grip_item = None
self.def_min_vetex = None
self.def_min_distance = None
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 release(self):
"""Release gripper object, only applied if gripper is 'activated'.
If suction off, detect contact between gripper and objects.
If suction on, detect contact between picked object and other objects.
To handle deformables, simply remove constraints (i.e., anchors).
Also reset any relevant variables, e.g., if releasing a rigid, we
should reset init_grip values back to None, which will be re-assigned
in any subsequent grasps.
"""
if self.activated:
self.activated = False
# Release gripped rigid object (if any).
if self.contact_constraint is not None:
try:
p.removeConstraint(self.contact_constraint)
self.contact_constraint = None
except: # pylint: disable=bare-except
pass
self.init_grip_distance = None
self.init_grip_item = None
# Release gripped deformable object (if any).
if self.def_grip_anchors:
for anchor_id in self.def_grip_anchors:
p.removeConstraint(anchor_id)
self.def_grip_anchors = []
self.def_grip_item = None
self.def_min_vetex = None
self.def_min_distance = None
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 remove_constraint(self, id):
""" Remove a constraint
"""
if id in self.constraint_id_map:
p.removeConstraint(self.constraint_id_map[id])
del self.constraint_id_map[id]
base_link_sim_id = self.entity_id_map[id]
p.changeDynamics(base_link_sim_id, -1, activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
def remove_constraints(self):
if (len(self.macroaction.link_status) > 0):
for obj1_index in range(len(self.objects)):
for obj2_index in range(obj1_index, len(self.objects)):
link_index = obj1_index * len(self.objects) + obj2_index
if (self.macroaction.links[link_index] is not None):
p.removeConstraint(self.macroaction.links[link_index])
self.macroaction.links[link_index] = None
def remove_constraint(self):
"""
Remove constraints created by create_constraint
"""
for cid in self.cid:
p.removeConstraint(cid)
self.cid = []
self.constraint_marker.set_position([0, 0, 100])
self.constraint_marker2.set_position([0, 0, 100])
def simple_simulate(self):
for parameter_count in range(1000):
indexes = cal_parameter_group(parameter_count)
spring_elastic_stiffness = 40.0
spring_damping_stiffness = 0.1
spring_bending_stiffness = 2.0
table_id, cloth_id, anchor_ids = self.load_world(
spring_elastic_stiffness, spring_damping_stiffness,
spring_bending_stiffness)
iteration = 0
iteration_name = "{:.1f}_{:.1f}_{:.1f}".format(
spring_elastic_stiffness, spring_damping_stiffness,
spring_bending_stiffness)
print("Saving file: ", iteration_name)
start = time.time()
for step in range(1150):
# suspend and release the cloth
if step == 0:
p.resetBaseVelocity(cloth_id, linearVelocity=[0, 0, 0.5])
# p.applyExternalForce(cloth_id, 0, [0, 0, 1000], [0, 0, 0], p.WORLD_FRAME)
elif step == 500:
p.resetBaseVelocity(cloth_id, linearVelocity=[0, 0, 0])
for anchor_id in anchor_ids:
p.removeConstraint(anchor_id)
# after releasing the cloth, record the data every 25 frame of simulation
if step >= 50 and step % 50 == 0:
for camera_position, eye_position in enumerate(
self.available_eye_positions)[3]:
target_position = p.getBasePositionAndOrientation(
cloth_id)[0]
view_matrix = p.computeViewMatrix(
cameraEyePosition=eye_position,
cameraTargetPosition=target_position,
cameraUpVector=self.up_vector)
projection_matrix = p.computeProjectionMatrixFOV(
fov=90, aspect=1.0, nearVal=0.01, farVal=5.1)
camera_image = p.getCameraImage(
width=self.width,
height=self.height,
viewMatrix=view_matrix,
projectionMatrix=projection_matrix,
renderer=self.renderer)
file_name = iteration_name + '_' + str(
iteration) + '_' + str(camera_position)
thread = Thread(target=save,
args=(file_name, camera_image))
thread.start()
time.sleep(
0.05
) # force the program sleep to avoid too many threads run at the same time
iteration += 1
print("finish saving in ", time.time() - start)
p.stepSimulation()
def removeConstraint(constraints, obj1, obj2):
"""
Remove constraint between two objects
:params:
constraints - current dictionary of constraints
obj1 - constrained object
obj2 - target object to which obj1 is constrained
"""
if obj1 in constraints.keys():
p.removeConstraint(constraints[obj1][1])
def remove_constraint(self, constraint_uid):
"""Remove a constraint.
Args:
constraint_uid: The constraint unique ID.
"""
# TODO(kuanfang): removeConstraint dose not work.
pybullet.changeConstraint(constraint_uid,
maxForce=0,
physicsClientId=self.uid)
pybullet.removeConstraint(constraint_uid, physicsClientId=self.uid)
def detach(self, object):
"""
This method detaches an other object from this object. After the detachment the detachment event of the
corresponding BulletWorld w:ill be fired.
:param object: The object which should be detached
"""
if object not in self.attachments:
return
p.removeConstraint(self.attachments[object][0])
del self.attachments[object]
del object.attachments[self]
self.world.detachment_event(self, [self, object])
def delete_constraint(self, constraintId):
if constraintId in self.constraints:
constraint = self.constraints[constraintId]
if constraintId in self.constraint_deletion_cbs:
for cb in self.constraint_deletion_cbs[constraintId]:
cb(self, constraintId, constraint)
del self.constraint_deletion_cbs[constraintId]
pb.removeConstraint(constraint.bId(), self.__client_id)
del self.__cId_IdMap[constraint.bId()]
del self.constraints[constraintId]
return True
return False
def detach(self, object):
"""
This method detaches an other object from this object. This is done by
deleting the attachment from the attachments dictionaty of both obejects
and deleting the constraint of pybullet.
After the detachment the detachment event of the
corresponding BulletWorld w:ill be fired.
:param object: The object which should be detached
"""
del self.attachments[object]
del object.attachments[self]
p.removeConstraint(self.cids[object], physicsClientId=self.world.client_id)
del self.cids[object]
del object.cids[self]
self.world.detachment_event(self, [self, object])
def undock(self,other):
'''
This undocks the two modules that is self->bot and other->bot
'''
ind = None
try:
ind = self.dockees.index(other)
except:
raise Exception(str(self.id)+" "+str(other.id )+", These two modules have not been docked before")
# print(self.id, other.id, "Are not docked before")
finally:
if ind is not None:
cid = self.constraints.pop(ind)
other.constraints.remove(cid)
p.removeConstraint(cid,physicsClientId=self.pClient)
self.dockees.remove(other)
other.dockees.remove(self)
def restoreOnInput(world_states, x1, y1, o1, constraints):
"""
Restore to last saved state when this function is called
"""
print(world_states)
if len(world_states) != 0:
world_states.pop()
id1, x, y, o, cids_old = world_states[-1]
cids_list_old = []
for obj in cids_old.keys():
cids_list_old.append(cids_old[obj][1])
for obj in constraints.keys():
if not constraints[obj][1] in cids_list_old:
p.removeConstraint(constraints[obj][1])
del (constraints[obj])
p.restoreState(stateId=id1)
# q=p.getQuaternionFromEuler((0,0,0))
# p.resetBasePositionAndOrientation(([0, 0, 0], q)) # Get robot to home when undo
# return 0, 0, 0, world_states
return x, y, o, constraints, world_states
return x1, y1, o1, constraints, world_states
def reset(self):
self._setup_ee()
self._open_gripper()
if self.gripperConstraint:
p.removeConstraint(self.gripperConstraint)
p.resetBasePositionAndOrientation(self.gripperId, self.initGripperPos,
self.endEffectorOrientation)
self.gripperConstraint = p.createConstraint(
self.gripperId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 1], [0, 0, 0],
self.initGripperPos, self.endEffectorOrientation)
self._close_gripper()
# set robot init config
self._clear_state_between_control_steps()
# start at rest
for _ in range(1000):
p.stepSimulation()
self.state = self._obs()
self._dd.draw_point('x0', self.get_ee_pos(self.state), color=(0, 1, 0))
return np.copy(self.state)
def reset(self):
# reset robot to nominal pose
p.resetBasePositionAndOrientation(self.pusherId, self.initPusherPos,
[0, 0, 0, 1])
p.resetBasePositionAndOrientation(
self.blockId, self.initBlockPos,
p.getQuaternionFromEuler([0, 0, self.initBlockYaw]))
# set robot init config
if self.pusherConstraint:
p.removeConstraint(self.pusherConstraint)
self.pusherConstraint = p.createConstraint(self.pusherId, -1, -1, -1,
p.JOINT_FIXED, [0, 0, 1],
[0, 0, 0],
self.initPusherPos)
self._clear_state_between_control_steps()
# start at rest
for _ in range(1000):
p.stepSimulation()
self.state = self._obs()
self._dd.draw_2d_pose('x0',
self.get_block_pose(self.state),
color=(0, 1, 0))
return np.copy(self.state)
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class, loads the robot into
the simulated instance. This method also updates the max velocity of
the robot's fingers, adds self collision filters to the model and
defines the cameras of the model in the camera_dict.
Parameters:
translation - List containing 3 elements, the translation [x, y, z]
of the robot in the WORLD frame
quaternion - List containing 4 elements, the quaternion
[x, y, z, q] of the robot in the WORLD frame
physicsClientId - The id of the simulated instance in which the
robot is supposed to be loaded
"""
pybullet.setAdditionalSearchPath(os.path.dirname(
os.path.realpath(__file__)),
physicsClientId=physicsClientId)
# Add 0.50 meters on the z component, avoing to spawn NAO in the ground
translation = [translation[0], translation[1], translation[2] + 1.05]
RobotVirtual.loadRobot(self,
translation,
quaternion,
physicsClientId=physicsClientId)
balance_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=translation,
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.goToPosture("Stand", 1.0)
pybullet.setCollisionFilterPair(self.robot_model,
self.robot_model,
self.link_dict["REye"].getIndex(),
self.link_dict["LEye"].getIndex(),
0,
physicsClientId=self.physics_client)
for link in ["torso", "HeadRoll_link"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["NeckPitch_link"].getIndex(),
self.link_dict[link].getIndex(),
0,
physicsClientId=self.physics_client)
for side in ["R", "L"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Eye"].getIndex(),
self.link_dict["HeadRoll_link"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Thigh"].getIndex(),
self.link_dict["body"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side.lower() + "_ankle"].getIndex(),
self.link_dict[side + "Tibia"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "ShoulderYaw_link"].getIndex(),
self.link_dict["torso"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "WristRoll_link"].getIndex(),
self.link_dict[side.lower() + "_wrist"].getIndex(),
0,
physicsClientId=self.physics_client)
for link in ["ShoulderYaw_link", "WristYaw_link"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + link].getIndex(),
self.link_dict[side + "Elbow"].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
if side + "Finger" in name or\
side + "Thumb" in name:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side.lower() + "_wrist"].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
for joint in self.joint_dict.values():
pybullet.resetJointState(self.robot_model,
joint.getIndex(),
0.0,
physicsClientId=self.physics_client)
pybullet.removeConstraint(balance_constraint,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
camera_right = CameraRgb(
self.robot_model,
RomeoVirtual.ID_CAMERA_RIGHT,
self.link_dict["CameraRightEye_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
camera_left = CameraRgb(self.robot_model,
RomeoVirtual.ID_CAMERA_LEFT,
self.link_dict["CameraLeftEye_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
camera_depth = CameraDepth(self.robot_model,
RomeoVirtual.ID_CAMERA_DEPTH,
self.link_dict["CameraDepth_optical_frame"],
hfov=58.0,
vfov=45.0,
physicsClientId=self.physics_client)
self.camera_dict = {
RomeoVirtual.ID_CAMERA_RIGHT: camera_right,
RomeoVirtual.ID_CAMERA_LEFT: camera_left,
RomeoVirtual.ID_CAMERA_DEPTH: camera_depth
}
# The frequency of the IMU is set to 100Hz
self.imu = Imu(self.robot_model,
self.link_dict["torso"],
100.0,
physicsClientId=self.physics_client)
def remove_all_constraints():
for cid in get_constraint_ids():
p.removeConstraint(cid)
def release_hold(self, armname):
p.removeConstraint(self.grasped[armname])
def remove_suction_force(self, cons):
p.removeConstraint(cons)
for i in range(10):
p.stepSimulation()
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])
def remove_constraint(constraint):
p.removeConstraint(constraint, physicsClientId=CLIENT)
def remove_constraint(self, id):
if id not in self.constraint_id_map:
raise ValueError(
"Constraint for entity <{}> not created".format(id))
p.removeConstraint(self.constraint_id[id])
def simulate(self):
"""
simulate the environment using pybullet and save file
:return:
"""
for speed in np.arange(5, 10):
for rand in range(10):
for spring_elastic_stiffness in self.elastic_stiffness_range:
spring_damping_stiffness = 0.1
spring_bending_stiffness = 2.0
table_id, cloth_id, anchor_ids = self.load_world(
spring_elastic_stiffness,
spring_damping_stiffness,
spring_bending_stiffness,
rand_pos=True)
iteration = 0
iteration_name = "{:.1f}_{:.1f}_{:.1f}".format(
spring_elastic_stiffness, spring_damping_stiffness,
spring_bending_stiffness)
print("Saving file: ", iteration_name, ', speed: ', speed,
', rand: ', rand)
start = time.time()
for step in range(1000):
# suspend and release the cloth
if step == 0:
# p.createSoftBodyAnchor(cloth_id, 1, -1, -1)
p.resetBaseVelocity(
cloth_id, linearVelocity=[0, 0, speed / 10])
# p.applyExternalForce(cloth_id, 1, [0, 0, 1000], [0, 0, 100], p.WORLD_FRAME)
elif step == 450:
p.resetBaseVelocity(cloth_id,
linearVelocity=[0, 0, 0])
for anchor_id in anchor_ids:
p.removeConstraint(anchor_id)
# after releasing the cloth, record the data every 25 frame of simulation
if step > 500 and step % 20 == 0:
for camera_position, eye_position in enumerate(
self.available_eye_positions):
target_position = [
0.0, -0.1, 0.5
] # p.getBasePositionAndOrientation(cloth_id)[0]
view_matrix = p.computeViewMatrix(
cameraEyePosition=eye_position,
cameraTargetPosition=target_position,
cameraUpVector=self.up_vector)
projection_matrix = p.computeProjectionMatrixFOV(
fov=45.0,
aspect=1.0,
nearVal=0.01,
farVal=5.1)
# p.computeViewMatrixFromYawPitchRoll()
camera_image = p.getCameraImage(
width=self.width,
height=self.height,
viewMatrix=view_matrix,
projectionMatrix=projection_matrix,
renderer=self.renderer)
file_name = '{}_{}_{}_{}_{}'.format(
iteration_name, iteration, camera_position,
speed, rand)
thread = Thread(target=save,
args=(file_name, camera_image))
thread.start()
time.sleep(
0.05
) # force the program sleep to avoid too many threads run at the same time
iteration += 1
print("finish saving in ", time.time() - start)
p.stepSimulation()
def remove(self):
pb.removeConstraint(self._constraint_id, physicsClientId=self.client_id)
def deconstrain(self):
if (not self.training):
p.removeConstraint(self.knot)
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(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])
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class, loads the robot into
the simulated instance. This method also updates the max velocity of
the robot's fingers, adds self collision filters to the model and
defines the cameras of the model in the camera_dict.
Parameters:
translation - List containing 3 elements, the translation [x, y, z]
of the robot in the WORLD frame
quaternion - List containing 4 elements, the quaternion
[x, y, z, q] of the robot in the WORLD frame
physicsClientId - The id of the simulated instance in which the
robot is supposed to be loaded
"""
pybullet.setAdditionalSearchPath(
os.path.dirname(os.path.realpath(__file__)),
physicsClientId=physicsClientId)
# Add 0.36 meters on the z component, avoing to spawn NAO in the ground
translation = [translation[0], translation[1], translation[2] + 0.36]
RobotVirtual.loadRobot(
self,
translation,
quaternion,
physicsClientId=physicsClientId)
balance_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=translation,
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.goToPosture("Stand", 1.0)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict["Head"].getIndex(),
0,
physicsClientId=self.physics_client)
for side in ["R", "L"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Thigh"].getIndex(),
self.link_dict[side + "Hip"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Bicep"].getIndex(),
self.link_dict[side + "ForeArm"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Pelvis"].getIndex(),
self.link_dict[side + "Thigh"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Tibia"].getIndex(),
self.link_dict[side.lower() + "_ankle"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Finger11_link"].getIndex(),
self.link_dict[side + "Finger13_link"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Finger21_link"].getIndex(),
self.link_dict[side + "Finger23_link"].getIndex(),
0,
physicsClientId=self.physics_client)
for base_link in ["RThigh", "LThigh", "RBicep", "LBicep"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[base_link].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
for wrist in ["r_wrist", "l_wrist"]:
if wrist[0] + "finger" in name.lower() or\
wrist[0] + "thumb" in name.lower():
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[wrist].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
for joint in self.joint_dict.values():
pybullet.resetJointState(
self.robot_model,
joint.getIndex(),
0.0,
physicsClientId=self.physics_client)
pybullet.removeConstraint(
balance_constraint,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
camera_top = CameraRgb(
self.robot_model,
NaoVirtual.ID_CAMERA_TOP,
self.link_dict["CameraTop_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
camera_bottom = CameraRgb(
self.robot_model,
NaoVirtual.ID_CAMERA_BOTTOM,
self.link_dict["CameraBottom_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
self.camera_dict = {
NaoVirtual.ID_CAMERA_TOP: camera_top,
NaoVirtual.ID_CAMERA_BOTTOM: camera_bottom}
# The frequency of the IMU is set to 100Hz
self.imu = Imu(
self.robot_model,
self.link_dict["torso"],
100.0,
physicsClientId=self.physics_client)
# FSRs
fsr_dict = dict()
for name in NaoFsr.LFOOT + NaoFsr.RFOOT:
fsr_dict[name] = Fsr(
self.robot_model,
self.link_dict[name],
physicsClientId=self.physics_client)
self._setFsrHandler(FsrHandler(fsr_dict))
import pybullet as p
import time
import math
from pbsbtest.utils import res_pkg_path
p.connect(p.GUI)
p.loadURDF(res_pkg_path("package://pbsbtest/urdf/plane.urdf"))
cubeId = p.loadURDF(res_pkg_path("package://pbsbtest/urdf/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)
def loadRobot(self, translation, quaternion, physicsClientId=0):
"""
Overloads @loadRobot from the @RobotVirtual class. Update max velocity
for the fingers and thumbs, based on the hand joints. Add self
collision exceptions. Add the cameras.
"""
pybullet.setAdditionalSearchPath(os.path.dirname(
os.path.realpath(__file__)),
physicsClientId=physicsClientId)
# Add 0.36 meters on the z component, avoing to spawn NAO in the ground
translation = [translation[0], translation[1], translation[2] + 0.36]
RobotVirtual.loadRobot(self,
translation,
quaternion,
physicsClientId=physicsClientId)
balance_constraint = pybullet.createConstraint(
parentBodyUniqueId=self.robot_model,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=pybullet.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
parentFrameOrientation=[0, 0, 0, 1],
childFramePosition=translation,
childFrameOrientation=quaternion,
physicsClientId=self.physics_client)
self.goToPosture("Stand", 1.0)
pybullet.setCollisionFilterPair(self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict["Head"].getIndex(),
0,
physicsClientId=self.physics_client)
for side in ["R", "L"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Thigh"].getIndex(),
self.link_dict[side + "Hip"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Bicep"].getIndex(),
self.link_dict[side + "ForeArm"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Pelvis"].getIndex(),
self.link_dict[side + "Thigh"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Tibia"].getIndex(),
self.link_dict[side.lower() + "_ankle"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Finger11_link"].getIndex(),
self.link_dict[side + "Finger13_link"].getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[side + "Finger21_link"].getIndex(),
self.link_dict[side + "Finger23_link"].getIndex(),
0,
physicsClientId=self.physics_client)
for base_link in ["RThigh", "LThigh", "RBicep", "LBicep"]:
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict["torso"].getIndex(),
self.link_dict[base_link].getIndex(),
0,
physicsClientId=self.physics_client)
for name, link in self.link_dict.items():
for wrist in ["r_wrist", "l_wrist"]:
if wrist[0] + "finger" in name.lower() or\
wrist[0] + "thumb" in name.lower():
pybullet.setCollisionFilterPair(
self.robot_model,
self.robot_model,
self.link_dict[wrist].getIndex(),
link.getIndex(),
0,
physicsClientId=self.physics_client)
pybullet.removeConstraint(balance_constraint,
physicsClientId=self.physics_client)
for joint_name in list(self.joint_dict):
if 'RFinger' in joint_name or 'RThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["RHand"].getMaxVelocity())
elif 'LFinger' in joint_name or 'LThumb' in joint_name:
self.joint_dict[joint_name].setMaxVelocity(
self.joint_dict["LHand"].getMaxVelocity())
self.camera_top = CameraRgb(self.robot_model,
self.link_dict["CameraTop_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)
self.camera_bottom = CameraRgb(
self.robot_model,
self.link_dict["CameraBottom_optical_frame"],
hfov=60.9,
vfov=47.6,
physicsClientId=self.physics_client)