def set_gripper_properties(self,
collidable=False,
measureable=False,
renderable=False,
detectable=False,
cuttable=False,
dynamic=False,
respondable=False,
visible=False):
"""Sets misc. parameters of the gripper model in the sim.
This is used to accomplish things such as: moving the gripper without
colliding with anything, setting it to be visible & being captured
by the cameras, static so fingers don't move, etc ...
"""
props = [
collidable, measureable, renderable, detectable, cuttable, dynamic,
respondable, visible
]
# V-REP encodes these properties as 'not_xxxxx', so we'll just invert
# them here to make calls in the simulator straightforward
props = [not p for p in props]
r = vrep.simxCallScriptFunction(self.clientID,
'remoteApiCommandServer',
vrep.sim_scripttype_childscript,
'setGripperProperties', props, [], [],
bytearray(), vrep.simx_opmode_blocking)
if r[0] != vrep.simx_return_ok:
raise Exception('Error setting gripper properties. Return code ',
r)
def get_contacts_info_for_other(self):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_GetContactsInfoForOther", [], [],
[], bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in getting contactsInfoForOther: {}".format(
res[0]))
return False
num_contacts_total = int(res[1][0])
num_contacts_other = int(res[1][1])
# parse the contact info
if num_contacts_other > 0:
assert len(res[2]) > 0
info_per_contact = len(res[2]) / num_contacts_other
all_contact_data = np.array(res[2]).reshape(num_contacts_other, -1)
logging.info("==== FOUND CONTACT ====: contacts ret: {} "
"parsed: {}, info_per_contact: {}".format(
num_contacts_total, num_contacts_other,
all_contact_data.shape[1]))
else:
logging.info(" ===== NO CONTACT =====")
all_contact_data = None
return all_contact_data
def get_all_objects_info(self):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_GetAllObjectsInfo", [], [], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in apply force on object: {}".format(res[0]))
return False
# Parse return values
assert len(res[2]) == 72, "Got invalid number of return values"
d = {}
d['anchor_pos'] = res[2][:3]
d['anchor_q'] = res[2][3:6]
d['anchor_v'] = res[2][6:9]
d['anchor_ang_v'] = res[2][9:12]
d['other_pos'] = res[2][12:15]
d['other_q'] = res[2][15:18]
d['other_v'] = res[2][18:21]
d['other_ang_v'] = res[2][21:24]
d['anchor_bb'] = res[2][24:30]
d['other_bb'] = res[2][30:36]
d['anchor_T_matrix'] = res[2][36:48]
d['other_T_matrix'] = res[2][48:60]
d['joint_pos'] = res[2][60:66]
d['joint_target_pos'] = res[2][66:72]
return True, d
def generate_object(self,
name,
shape_type,
size,
pos,
q,
color,
mass,
lin_damp,
ang_damp,
is_static=0):
'''
shape_type: Int. 0 for a cuboid, 1 for a sphere, 2 for a cylinder and
3 for a cone.
'''
assert shape_type >= 0 and shape_type <= 3
assert len(size) == 3, "Invalid size object"
float_args = [
# Size - 0, 1, 2
size[0],
size[1],
size[2],
# pos - 3, 4, 5
pos[0],
pos[1],
pos[2],
# orientation - 6, 7, 8
# q[0], q[1], q[2],
0,
0,
0,
# color - 9, 10, 11
color[0],
color[1],
color[2],
# mass - 12 mass,
# linear damp, angular damp - 13, 14
lin_damp,
ang_damp
]
assert is_static in (0, 1), "Invalid values for is_static"
res = vrep.simxCallScriptFunction(
self.client_id,
"WorldCreator",
vrep.sim_scripttype_childscript,
"ms_AddAnchorBlock",
[shape_type, is_static],
float_args,
[name],
bytearray(),
vrep.simx_opmode_blocking,
)
assert res[0] == vrep.simx_return_ok, "Did not create object"
obj_handle = res[1][0]
logging.debug("Created object with handle: {}".format(obj_handle))
return obj_handle
def apply_force(self, position, force):
res = vrep.simxCallScriptFunction(
self.client_id, "Cuboid", vrep.sim_scripttype_childscript,
"ms_ApplyForce", [int(self.handles_dict['other_obj'])], [
position[0], position[1], position[2], force[0], force[1],
force[2]
], [], bytearray(), vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
print("Error in apply force: {}".format(res[0]))
def remove_anchor_object(self):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_RemoveAnchorObject", [], [], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in remove_anchor_object: {}".format(res[0]))
return False
return True
def update_keep_object_in_place(self):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_KeepObjectInPlace", [], [], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in update_keep_in_place: {}".format(res[0]))
return False
return True
def set_joint_pid_values(self, p, i, d):
res = vrep.simxCallScriptFunction(self.client_id, "RobotBase",
vrep.sim_scripttype_childscript,
"ms_SetPIDValues", [], [p, i, d], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in set_joint_pid_values: {}".format(res[0]))
return False
return True
def apply_force_on_object(self, o_handle, force):
res = vrep.simxCallScriptFunction(
self.client_id, "WorldCreator", vrep.sim_scripttype_childscript,
"ms_ApplyForceOnObject", [int(self.handles_dict['other_obj'])],
[force[0], force[1], force[2]], [], bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in apply force on object: {}".format(res[0]))
return False
return True
def free_other_object(self):
res = vrep.simxCallScriptFunction(self.client_id, "RobotBase",
vrep.sim_scripttype_childscript,
"ms_freeOtherObject", [], [], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in free_other_object: {}".format(res[0]))
return False
return True
def set_object_damping_param(self, obj_handle, lin_damp, ang_damp):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_SetEngineParam",
[o_handle], [lin_damp, ang_damp], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in setting damping param: {}".format(res[0]))
return False
return True
def update_add_object(self, anchor_or_other_obj):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_DidAddObject",
[anchor_or_other_obj], [], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in updateAddObject: {}".format(res[0]))
return False
return True
def apply_force(self, o_handle, position, force):
res = vrep.simxCallScriptFunction(
self.client_id, "WorldCreator", vrep.sim_scripttype_childscript,
"ms_ApplyForce", [o_handle], [
position[0], position[1], position[2], force[0], force[1],
force[2]
], [], bytearray(), vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in apply force: {}".format(res[0]))
return False
return True
def toggle_record_ft_data(self):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_toggleRecordingFTData", [], [],
[], bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in ms_toggleRecordingFTData: {}".format(
res[0]))
return False
return True
def apply_force_in_ref_frame(self, force):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_ApplyForceInWorldFrameAtCOM", [],
[force[0], force[1], force[2]], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in apply_force_in_ref_frame: {}".format(
res[0]))
return False
return True
def save_record_ft_data(self):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_saveRecordedFTData", [], [], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in ms_saveRecordedFtData: {}".format(res[0]))
return None
all_ft_data = res[2]
return all_ft_data
def stop_object_controller(self, obj_handle, dt):
'''Apply force and torque to stop object motion.'''
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_stopObjectController",
[obj_handle], [dt], [], bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in apply force: {}".format(res[0]))
return False
force, torque = res[2][0:3], res[2][3:]
return force, torque
def get_object_damping_param(self, obj_handle):
'''Get object damping parameters.'''
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_GetEngineParam", [obj_handle],
[], [], bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in getting damping param: {}".format(res[0]))
return False
lin_damp, ang_damp = res[2][0], res[2][1]
return lin_damp, ang_damp
def update_octree_position(self, pos):
assert len(pos) == 3, "Invalid position to be set"
res = vrep.simxCallScriptFunction(self.client_id, "Octree",
vrep.sim_scripttype_childscript,
"ms_UpdateOctreePosition", [],
[pos[0], pos[1], pos[2]], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in update_octree_position: {}".format(res[0]))
return False
return True
def set_joint_position_by_name(self, name, position):
"""Given a name of a joint, get the current position"""
r = vrep.simxCallScriptFunction(self.clientID,
'remoteApiCommandServer',
vrep.sim_scripttype_childscript,
'setJointPositionByName', [],
[position], [name], bytearray(),
vrep.simx_opmode_blocking)
if r[0] != vrep.simx_return_ok:
raise Exception('Error setting joint position for <%s>!' % name,
'Return code ', r)
def set_pose_by_name(self, name, frame_work2pose):
"""Sets the pose of a scene object (by name) WRT workspace."""
frame = self._format_matrix(frame_work2pose)
r = vrep.simxCallScriptFunction(self.clientID,
'remoteApiCommandServer',
vrep.sim_scripttype_childscript,
'setPoseByName', [], frame, [name],
bytearray(), vrep.simx_opmode_blocking)
if r[0] != vrep.simx_return_ok:
raise Exception('Error setting pose for name <%s>!' % name,
'Return code ', r)
def get_joint_offsets(self):
'''Get joint offsets in the scene.
Performs joint_pos - joint_target_pos on each joint.
'''
res = vrep.simxCallScriptFunction(self.client_id, "RobotBase",
vrep.sim_scripttype_childscript,
"ms_GetJointOffsets", [], [], [],
bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in ms_GetJointOffsets: {}".format(res[0]))
return None
return res[2]
def update_octree_with_single_object(self, obj_handle):
res = vrep.simxCallScriptFunction(
self.client_id,
"Octree",
vrep.sim_scripttype_childscript,
"ms_UpdateOctreeWithSingleObject",
[obj_handle],
[],
[],
bytearray(),
vrep.simx_opmode_blocking,
)
if res[0] != vrep.simx_return_ok:
logging.error("Error in udpate octree: {}".format(res[0]))
def get_oriented_bounding_box(self):
res = vrep.simxCallScriptFunction(self.client_id, "WorldCreator",
vrep.sim_scripttype_childscript,
"ms_GetOrientedAbsoluteBoundingBox",
[], [], [], bytearray(),
vrep.simx_opmode_blocking)
if res[0] != vrep.simx_return_ok:
logging.error("Error in getting oriented bounding box: {}".format(
res[0]))
return False
assert len(res[2]) == 6, "Got invalid number of return values"
lb, ub = np.array(res[2][:3]), np.array(res[2][3:])
return lb, ub
def clear_octree(self):
res = vrep.simxCallScriptFunction(
self.client_id,
"Octree",
vrep.sim_scripttype_childscript,
"ms_ClearOctree",
[int(self.handles_dict['octree'])],
[],
[],
bytearray(),
vrep.simx_opmode_blocking,
)
if res[0] != vrep.simx_return_ok:
logging.error("Error in clear octree: {}".format(res[0]))
def get_object_distance(self):
res = vrep.simxCallScriptFunction(
self.client_id,
"WorldCreator",
vrep.sim_scripttype_childscript,
"ms_GetObjectDistance",
[],
[],
[],
bytearray(),
vrep.simx_opmode_blocking,
)
if res[0] != vrep.simx_return_ok:
logging.error("Error in get object distance: {}".format(res[0]))
return res[2]
def set_gripper_kinematics_mode(self, mode='forward'):
joint_modes = ['forward', 'inverse']
if mode not in joint_modes:
raise Exception('Joint mode must be in %s' % joint_modes)
r = vrep.simxCallScriptFunction(self.clientID,
'remoteApiCommandServer',
vrep.sim_scripttype_childscript,
'setJointKinematicsMode', [], [],
[mode], bytearray(),
vrep.simx_opmode_blocking)
if r[0] != vrep.simx_return_ok:
raise Exception('Error setting gripper kinematics mode.',
'Return code ', r)
def get_pose_by_name(self, name):
"""Queries the simulator for the pose of object corresponding to <name>.
The retrieved pose is with respect to the workspace frame.
"""
r = vrep.simxCallScriptFunction(self.clientID,
'remoteApiCommandServer',
vrep.sim_scripttype_childscript,
'getPoseByName', [], [], [name],
bytearray(), vrep.simx_opmode_blocking)
if r[0] != vrep.simx_return_ok:
raise Exception('Error getting pose for <%s>!' % name,
'Return code ', r)
return lib.utils.format_htmatrix(r[2])
def get_octree_voxels(self):
res = vrep.simxCallScriptFunction(
self.client_id,
"Octree",
vrep.sim_scripttype_childscript,
"ms_GetOctreeVoxels",
[int(self.handles_dict['octree'])],
[],
[],
bytearray(),
vrep.simx_opmode_blocking,
)
if res[0] != vrep.simx_return_ok:
logging.error("Error in getting octree voxels: {}".format(res[0]))
assert len(res[2]) > 0, "Empty octree voxels"
return res[2]
def get_force_vector_from_other_to_anchor(self):
# ms_GetForceVectorFromOtherToAnchor
res = vrep.simxCallScriptFunction(
self.client_id,
"WorldCreator",
vrep.sim_scripttype_childscript,
"ms_GetForceVectorFromOtherToAnchor",
[],
[],
[],
bytearray(),
vrep.simx_opmode_blocking,
)
if res[0] != vrep.simx_return_ok:
logging.error("Error in getting force vec: {}".format(res[0]))
print(np.array(res[2]))
return np.array(res[2])
