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])