コード例 #1
0
ファイル: mover.py プロジェクト: suhasjs/hops-framework
    def wait_till_stream(self):
        ret = vrep.simx_return_illegal_opmode_flag
        while ret != vrep.simx_return_ok:
            ret, ori = vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer)
            self.logger.info("orient:%s" % str(ori))

        ret = vrep.simx_return_illegal_opmode_flag
        while ret != vrep.simx_return_ok:
            ret, _ = vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer)

        ret = vrep.simx_return_illegal_opmode_flag
        while ret != vrep.simx_return_ok:
            ret, _ = vrep.simxGetJointPosition(self.client_id, self.left_joint, vrep.simx_opmode_buffer)

        ret = vrep.simx_return_illegal_opmode_flag
        while ret != vrep.simx_return_ok:
            ret, _ = vrep.simxGetJointPosition(self.client_id, self.right_joint, vrep.simx_opmode_buffer)

        ret = vrep.simx_return_illegal_opmode_flag
        while ret != vrep.simx_return_ok:
            ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer)

        ret = vrep.simx_return_illegal_opmode_flag
        while ret != vrep.simx_return_ok:
            ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer)
コード例 #2
0
    def get_vision_sensor_info_from_name(self, vision_sensor_name):
        
        #returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, vision_sensor_name, self.operation_mode)
        returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, vision_sensor_name, vrep.simx_opmode_oneshot_wait)
    
        # Skip non-existent objects
        if (returnCode != 0):
            return None

        returnCode, param_near_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1000, self.operation_mode)
        returnCode, param_far_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1001, self.operation_mode)
        returnCode, param_resolution_x = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1002, self.operation_mode)
        returnCode, param_resolution_y = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1003, self.operation_mode)
        returnCode, param_perspective_angle = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1004, self.operation_mode)

        try:
            ratio = param_resolution_x/param_resolution_y
        except ZeroDivisionError:
            return None

        if (ratio > 1):
            param_perspective_angle_x = param_perspective_angle
            param_perspective_angle_y = 2*np.arctan(np.tan(param_perspective_angle/2)/ratio)
        else:
            param_perspective_angle_x = 2*np.arctan(np.tan(param_perspective_angle/2)*ratio)
            param_perspective_angle_y = param_perspective_angle
        return VisionSensor(vision_sensor_name, \
                            param_resolution_x, \
                            param_resolution_y, \
                            param_perspective_angle, \
                            param_perspective_angle_x, \
                            param_perspective_angle_y, \
                            param_near_clipping, \
                            param_far_clipping, \
                            obj_handle)
コード例 #3
0
    def render(self, position):
        margin = -0.35
        # print ("robot pose: ", position)
        if self.robot_sim is None:
            # x, self.robot_sim = vrep.simxGetObjectHandle(self.clientID, "Quadricopter", vrep.simx_opmode_blocking)
            # x, self.robot_sim = vrep.simxLoadModel(self.clientID, self.models_path + "Quadricopter.ttm", 0, vrep.simx_opmode_blocking)

            x, self.robot_sim = vrep.simxGetObjectHandle(
                self.clientID, "youBot", vrep.simx_opmode_blocking)
            # x, self.target = vrep.simxLoadModel(self.clientID, self.models_path + "KUKA YouBot.ttm", 0, vrep.simx_opmode_blocking)

            vrep.simxSetObjectPosition(self.clientID, self.robot_sim, -1, (position[0], position[1], position[2] + margin + \
             math.fabs(vrep.simxGetObjectFloatParameter (self.clientID, self.robot_sim, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1])), vrep.simx_opmode_oneshot)

            #x, self.target = vrep.simxGetObjectHandle(self.clientID, "Quadricopter_target", vrep.simx_opmode_blocking) ##quad
            x, self.target = vrep.simxGetObjectHandle(
                self.clientID, "target", vrep.simx_opmode_blocking)
            vrep.simxSetObjectPosition(self.clientID, self.target, -1, (position[0], position[1], position[2] + margin + \
             math.fabs(vrep.simxGetObjectFloatParameter (self.clientID, self.robot_sim, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1])), vrep.simx_opmode_oneshot)

            x, self.target_hall = vrep.simxGetObjectHandle(
                self.clientID, "inflated_convex_hall_target",
                vrep.simx_opmode_blocking)
            # vrep.simxSetObjectPosition(self.clientID, self.target, -1, (position[0], position[1] + 1, position[2] + margin + \
            #	math.fabs(vrep.simxGetObjectFloatParameter (self.clientID, self.robot_sim, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1])), vrep.simx_opmode_oneshot)

            # vrep.simxSetObjectParent(self.clientID,self.target,-1,True, vrep.simx_opmode_blocking)
            # x, self.goal_robot = vrep.simxLoadModel(self.clientID, self.models_path + "KUKA YouBot.ttm", 0, vrep.simx_opmode_blocking)
            # x, self.goal_robot = vrep.simxGetObjectHandle(self.clientID, "Quadricopter#0", vrep.simx_opmode_blocking)
            # simxInt simxSetModelProperty(self.clientID,self.goal_robot,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
            ##sim.modelproperty_not_visible
        else:
            pass
コード例 #4
0
    def get_dynamic_obj_info(self):

        dynamic_obj_list = []

        for obj_name in self.dynamic_obj_name_list:
            #returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, obj_name, self.operation_mode)
            returnCode, obj_handle = vrep.simxGetObjectHandle(
                self.clientID, obj_name, vrep.simx_opmode_oneshot_wait)

            # Skip non-existent objects
            if (returnCode != 0):
                continue

            # Get pose and orientation
            returnCode, obj_pose = vrep.simxGetObjectPosition(
                self.clientID, obj_handle, -1, self.operation_mode)
            returnCode, obj_ori = vrep.simxGetObjectOrientation(
                self.clientID, obj_handle, -1, self.operation_mode)

            # Get the velocity of the obstacle
            returnCode, param_vel_x = vrep.simxGetObjectFloatParameter(
                self.clientID, obj_handle, 11, self.operation_mode)
            returnCode, param_vel_y = vrep.simxGetObjectFloatParameter(
                self.clientID, obj_handle, 12, self.operation_mode)
            returnCode, param_vel_z = vrep.simxGetObjectFloatParameter(
                self.clientID, obj_handle, 13, self.operation_mode)
            returnCode, param_vel_r = vrep.simxGetObjectFloatParameter(
                self.clientID, obj_handle, 14, self.operation_mode)

            obj_vel = np.array(
                [param_vel_x, param_vel_y, param_vel_z, param_vel_r])

        self.dynamic_obj_list = dynamic_obj_list
コード例 #5
0
    def retrieve_state(self):

        pos = np.zeros(2)
        vel = np.zeros(2)
        tip = np.zeros(3)

        _, pos[0] = vrep.simxGetJointPosition(self.clientID,
                                              self.jointHandles[0],
                                              vrep.simx_opmode_blocking)
        _, pos[1] = vrep.simxGetJointPosition(self.clientID,
                                              self.jointHandles[1],
                                              vrep.simx_opmode_blocking)
        _, vel[0] = vrep.simxGetObjectFloatParameter(self.clientID,
                                                     self.jointHandles[0],
                                                     2012,
                                                     vrep.simx_opmode_blocking)
        _, vel[1] = vrep.simxGetObjectFloatParameter(self.clientID,
                                                     self.jointHandles[1],
                                                     2012,
                                                     vrep.simx_opmode_blocking)
        _, tip_list = vrep.simxGetObjectPosition(self.clientID, self.tipHandle,
                                                 -1, vrep.simx_opmode_blocking)
        tip = np.asarray(tip_list)

        return {1: pos, 2: vel, 3: tip}
コード例 #6
0
    def get_vision_sensor_info_from_name(self, vision_sensor_name):
        
        returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, vision_sensor_name, self.operation_mode)

        if (returnCode == 0):
            returnCode, param_near_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1000, self.operation_mode)
            returnCode, param_far_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1001, self.operation_mode)
            returnCode, param_resolution_x = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1002, self.operation_mode)
            returnCode, param_resolution_y = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1003, self.operation_mode)
            returnCode, param_perspective_angle = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1004, self.operation_mode)

            ratio = param_resolution_x/param_resolution_y

            if (ratio > 1):
                param_perspective_angle_x = param_perspective_angle
                param_perspective_angle_y = 2*np.arctan(np.tan(param_perspective_angle/2)/ratio)
            else:
                param_perspective_angle_x = 2*np.arctan(np.tan(param_perspective_angle/2)*ratio)
                param_perspective_angle_y = param_perspective_angle

            return VisionSensor(vision_sensor_name, \
                                param_resolution_x, \
                                param_resolution_y, \
                                param_perspective_angle, \
                                param_perspective_angle_x, \
                                param_perspective_angle_y, \
                                param_near_clipping, \
                                param_far_clipping, \
                                obj_handle)
コード例 #7
0
    def update_dynamic_obj_info(self):

        for dynamic_obj in (self.dynamic_obj_list + self.robot_obj_list):
            obj_handle = dynamic_obj.handle 

            # Get pose and orientation 
            returnCode, obj_pose = vrep.simxGetObjectPosition(self.clientID, obj_handle, -1, self.operation_mode)
            returnCode, obj_ori = vrep.simxGetObjectOrientation(self.clientID, obj_handle, -1, self.operation_mode)
        
            # Get the velocity of the obstacle
            returnCode, param_vel_x = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 11, self.operation_mode)
            returnCode, param_vel_y = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 12, self.operation_mode)
            returnCode, param_vel_z = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 13, self.operation_mode)
            returnCode, param_vel_r = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 14, self.operation_mode)

            obj_vel = np.array([param_vel_x, param_vel_y, param_vel_z, param_vel_r])
            
            obj_size = dynamic_obj.size
   
            bbox_min = [obj_pose[0]-obj_size[0]/2.0, obj_pose[1]-obj_size[1]/2.0, obj_pose[2]-obj_size[2]/2.0]
            bbox_max = [obj_pose[0]+obj_size[0]/2.0, obj_pose[1]+obj_size[1]/2.0, obj_pose[2]+obj_size[2]/2.0]
       
            # TODO: must check if obj is really updating
            dynamic_obj.pose = obj_pose
            dynamic_obj.ori = obj_ori
            dynamic_obj.vel = obj_vel
            dynamic_obj.bbox_min = bbox_min
            dynamic_obj.bbox_max = bbox_max
コード例 #8
0
ファイル: robots.py プロジェクト: bjkomer/nengo_vrep
    def handle_output(self):
        # return whatever state information you need to get the error you want
        # this will get you the information for the center of the object. If you
        # want something like the position of the end of an arm, you will need
        # to do some calculations, or just make a dummy object, attach it to
        # the point you want, and get the position of the dummy object
        
        #err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1,
        #                                           vrep.simx_opmode_oneshot)
        #err, hand_ori = vrep.simxGetObjectOrientation(self.cid, self.hand, -1,
        #                                           vrep.simx_opmode_oneshot)
        err, hand_ori = vrep.simxGetJointPosition(self.cid, self.hand_joint,
                                                   vrep.simx_opmode_oneshot)
        err, arm_ori = vrep.simxGetJointPosition(self.cid, self.arm_joint,
                                                   vrep.simx_opmode_oneshot)
        err, hand_vel = vrep.simxGetObjectFloatParameter(self.cid, self.hand_joint,
                                                         2012, vrep.simx_opmode_oneshot)
        err, arm_vel = vrep.simxGetObjectFloatParameter(self.cid, self.arm_joint,
                                                        2012, vrep.simx_opmode_oneshot)
        err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1,
                                                   vrep.simx_opmode_oneshot)
        err, target_pos = vrep.simxGetObjectPosition(self.cid, self.target, -1,
                                                   vrep.simx_opmode_oneshot)

        return [arm_ori, hand_ori, arm_vel, hand_vel, hand_pos[0], hand_pos[2],
                target_pos[0], target_pos[1]]
コード例 #9
0
    def getWheelVelocity(self):
        err, left_w = vrep.simxGetObjectFloatParameter(
            self.clientID, self.motor_left,
            vrepConst.sim_jointfloatparam_velocity, vrep.simx_opmode_blocking)
        err, right_w = vrep.simxGetObjectFloatParameter(
            self.clientID, self.motor_right,
            vrepConst.sim_jointfloatparam_velocity, vrep.simx_opmode_blocking)

        return (left_w, right_w)
コード例 #10
0
    def get_info(self):
        # Check velocity
        err, bl_wheel_vel = vrep.simxGetObjectFloatParameter(self.clientID, self.bl_brake_handle, vrep.sim_jointfloatparam_velocity, vrep.simx_opmode_streaming);
        err, br_wheel_vel = vrep.simxGetObjectFloatParameter(self.clientID, self.br_brake_handle, vrep.sim_jointfloatparam_velocity, vrep.simx_opmode_streaming);
        rear_wheel_velocity = ((bl_wheel_vel) + (br_wheel_vel))/2.0;
        linear_velocity = rear_wheel_velocity * 0.09 * 3.6; # Kmph

        throttle = linear_velocity;
        steer_errorCode, steer_pos = vrep.simxGetJointPosition(self.clientID, self.steer_handle, vrep.simx_opmode_streaming);
        if(self.printFlag):
            print('Throttle:', throttle, 'Steering:', steer_pos);
コード例 #11
0
    def get_all_objects_info(self):
        
        obj_name_list = self.static_obj_name_list + self.dynamic_obj_name_list + self.robot_name_list
        obj_list = []

        for obj_name in obj_name_list:
    
            returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, obj_name, self.operation_mode)
    
            # Get pose and orientation 
            returnCode, obj_pose = vrep.simxGetObjectPosition(self.clientID, obj_handle, -1, self.operation_mode)
            returnCode, obj_ori = vrep.simxGetObjectOrientation(self.clientID, obj_handle, -1, self.operation_mode)
    
            # Get size
            returnCode, param_min_x = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 21, self.operation_mode)
            returnCode, param_min_y = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 22, self.operation_mode)
            returnCode, param_min_z = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 23, self.operation_mode)
            returnCode, param_max_x = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 24, self.operation_mode)
            returnCode, param_max_y = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 25, self.operation_mode)
            returnCode, param_max_z = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 26, self.operation_mode)
    
            size_x = param_max_x - param_min_x
            size_y = param_max_y - param_min_y
            size_z = param_max_z - param_min_z
    
            obj_size = np.array([size_x, size_y, size_z])
   
            bbox_min = [obj_pose[0]-size_x/2.0, obj_pose[1]-size_y/2.0, obj_pose[2]-size_z/2.0]
            bbox_max = [obj_pose[0]+size_x/2.0, obj_pose[1]+size_y/2.0, obj_pose[2]+size_z/2.0]

            # Get velocity
            returnCode, param_vel_x = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 11, self.operation_mode)
            returnCode, param_vel_y = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 12, self.operation_mode)
            returnCode, param_vel_z = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 13, self.operation_mode)
            returnCode, param_vel_r = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 14, self.operation_mode)
    
            obj_vel = np.array([param_vel_x, param_vel_y, param_vel_z, param_vel_r])
   
            # Add object to static, dynamic or robot list 
            if (obj_name in self.static_obj_name_list):
                obj = SceneObject(obj_name, obj_pose, obj_ori, obj_size, obj_vel, bbox_min, bbox_max, obj_handle)
                self.static_obj_list.append(obj)
            elif (obj_name in self.dynamic_obj_name_list):
                obj = SceneObject(obj_name, obj_pose, obj_ori, obj_size, obj_vel, bbox_min, bbox_max, obj_handle)
                self.dynamic_obj_list.append(obj)
            else:
                obj = Robot(obj_name, obj_pose, obj_ori, obj_size, obj_vel, bbox_min, bbox_max, obj_handle)
                self.get_robot_vision_sensor_info(obj)
                self.robot_obj_list.append(obj)
        
        obj_list = self.static_obj_list + self.dynamic_obj_list + self.robot_obj_list
        
        return obj_list
コード例 #12
0
ファイル: hexapod.py プロジェクト: suhasjs/hops-framework
 def signal_values(self):
     vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1,
                                   vrep.simx_opmode_streaming)
     vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1,
                                vrep.simx_opmode_streaming)
     for handle_idx in range(0, self.joint_count):
         vrep.simxGetObjectFloatParameter(self.client_id,
                                          self.handles[handle_idx], 2012,
                                          vrep.simx_opmode_streaming)
     for handle_idx in range(0, self.joint_count):
         vrep.simxGetJointPosition(self.client_id, self.handles[handle_idx],
                                   vrep.simx_opmode_streaming)
コード例 #13
0
    def render(self, table, sim_env):
        position = [
            table.get_feature("x").value - env_margin,
            table.get_feature("y").value - env_margin, 0
        ]

        x, sim_table = vrep.simxLoadModel(self.clientID,
                                          self.models_path + "diningTable.ttm",
                                          0, vrep.simx_opmode_blocking)
        # x = vrep.simxSetObjectParent(self.clientID, sim_table, sim_env, True, vrep.simx_opmode_blocking)
        vrep.simxSetObjectPosition(self.clientID, sim_table, -1, (position[0], position[1], position[2] + \
         vrep.simxGetObjectFloatParameter (self.clientID, sim_table, vrep.sim_objfloatparam_modelbbox_max_x, vrep.simx_opmode_blocking)[1]), vrep.simx_opmode_oneshot)
        # vrep.simxSetModelProperty(self.clientID,sim_table,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)

        #returnCode, prop = vrep.simxGetModelProperty(self.clientID,sim_table,vrep.simx_opmode_blocking)
        #returnCode2 = vrep.simxSetModelProperty(self.clientID, sim_table, prop + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking)
        #returnCode3, prop = vrep.simxGetModelProperty(self.clientID, sim_table, vrep.simx_opmode_blocking)

        #print (prop)

        offset_z = math.fabs(
            vrep.simxGetObjectFloatParameter(
                self.clientID, sim_table,
                vrep.sim_objfloatparam_modelbbox_min_z,
                vrep.simx_opmode_blocking)[1])
        offset_y = math.fabs(
            vrep.simxGetObjectFloatParameter(
                self.clientID, sim_table,
                vrep.sim_objfloatparam_modelbbox_min_y,
                vrep.simx_opmode_blocking)[1])
        margin = 0.2
        bill_margin = 0.05

        self.chairs_offset = [(0, offset_y + margin, 0, 0, 0, 0),
                              (0, -offset_y - margin, 0, 0, 0, math.pi),
                              (offset_z + margin, 0, 0, 0, 0, -math.pi / 2),
                              (-offset_z - margin, 0, 0, 0, 0, math.pi / 2)]
        self.humans_offset = [
            (0, offset_y - margin, bill_margin, 0, 0, -math.pi / 2),
            (0, -offset_y + margin, bill_margin, 0, 0, math.pi / 2),
            (offset_z - margin, 0, bill_margin, 0, 0, math.pi),
            (-offset_z + margin, 0, bill_margin, 0, 0, 0)
        ]

        for i in range(len(table.humans)):
            sim_chair = self.render_chair(sim_table, position,
                                          self.chairs_offset[i])
            self.chairs.append(sim_chair)
            self.humans.append(
                self.render_human(sim_chair, position, self.humans_offset[i]))

        self.sim_table = sim_table
        return sim_table
コード例 #14
0
def getVirtualCamAdditionalMatrix(sensorHandle):
    "获得深度相机的近景/远景距离/视场角/分辨率"
    err, nearClip = vrep.simxGetObjectFloatParameter(
        clientID, sensorHandle, vrep.sim_visionfloatparam_near_clipping, vrep.simx_opmode_oneshot_wait)
    err, farClip = vrep.simxGetObjectFloatParameter(
        clientID, sensorHandle, vrep.sim_visionfloatparam_far_clipping, vrep.simx_opmode_oneshot_wait)
    err, angle = vrep.simxGetObjectFloatParameter(
        clientID, sensorHandle, vrep.sim_visionfloatparam_perspective_angle, vrep.simx_opmode_oneshot_wait)
    err, res_x = vrep.simxGetObjectIntParameter(
        clientID, sensorHandle, vrep.sim_visionintparam_resolution_x, vrep.simx_opmode_oneshot_wait)
    err, res_y = vrep.simxGetObjectIntParameter(
        clientID, sensorHandle, vrep.sim_visionintparam_resolution_y, vrep.simx_opmode_oneshot_wait)
    return nearClip, farClip, angle, (res_x, res_y)
コード例 #15
0
def get_maze_segments(wallHandles):

    ## Get handles to:
    # 1. The Maze collection:
    res, mazeHandle = vrep.simxGetCollectionHandle(clientID, "Maze",
                                                   vrep.simx_opmode_blocking)

    # Get the handles associated with each wall and the absolute position of the center of each wall:
    res, wallHandles, intData, absPositions, stringData = vrep.simxGetObjectGroupData(
        clientID, mazeHandle, 3, vrep.simx_opmode_blocking)

    mazeSegments = []

    if res == vrep.simx_return_ok:

        count = 1
        for wall in wallHandles:

            res, wallCenterAbsPos = vrep.simxGetObjectPosition(
                clientID, wall, -1, vrep.simx_opmode_oneshot_wait)

            res, wallMinY = vrep.simxGetObjectFloatParameter(
                clientID, wall, vrep.sim_objfloatparam_objbbox_min_y,
                vrep.simx_opmode_oneshot_wait)

            res, wallMaxY = vrep.simxGetObjectFloatParameter(
                clientID, wall, vrep.sim_objfloatparam_objbbox_max_y,
                vrep.simx_opmode_oneshot_wait)

            wallLength = abs(wallMaxY - wallMinY)

            # Get the orientation of the wall: Third euler angle is the angle around z-axis:
            res, wallOrient = vrep.simxGetObjectOrientation(
                clientID, wall, -1, vrep.simx_opmode_oneshot_wait)

            # Get the end points of the maze wall: A list containing two tuples: [ (xs,ys) , (xe,ye)]
            wallSeg = get_wall_seg(
                wallCenterAbsPos, wallLength, wallOrient[2]
            )  # Assuming all walls are on the ground and nearly flat:

            print("Wall #", count, " -> ", wallSeg)

            mazeSegments.append(wallSeg)

            count += 1

    else:

        print(" Failed to get individual wall handles!")

    return mazeSegments
コード例 #16
0
    def render_human(self, chair, position, offset):
        x, person = vrep.simxLoadModel(self.clientID,
                                       self.models_path + "Sitting Bill.ttm",
                                       0, vrep.simx_opmode_blocking)
        x = vrep.simxSetObjectParent(self.clientID, person, chair, True,
                                     vrep.simx_opmode_blocking)
        offset_z = vrep.simxGetObjectFloatParameter(
            self.clientID, person, vrep.sim_objfloatparam_modelbbox_max_z,
            vrep.simx_opmode_blocking)[1]
        vrep.simxSetObjectPosition(self.clientID, person, -1,
                                   (position[0] + offset[0], position[1] +
                                    offset[1], position[2] + offset[2]),
                                   vrep.simx_opmode_oneshot)
        vrep.simxSetObjectOrientation(self.clientID, person, -1,
                                      (offset[3], offset[4], offset[5]),
                                      vrep.simx_opmode_oneshot)
        # vrep.simxSetModelProperty(self.clientID,person,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)

        # x, objs = vrep.simxGetObjects(self.clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait)
        # x, joint = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftElbowJoint', vrep.simx_opmode_oneshot_wait)
        # x, shoulder = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftShoulderJoint', vrep.simx_opmode_oneshot_wait)
        # rotationMatrix = (-1,0,0,0,0,-1,0,0,0,0,-1,0)
        # x = vrep.simxSetSphericalJointMatrix(self.clientID, shoulder, rotationMatrix,vrep.simx_opmode_streaming)

        #Moving Rotational Joint
        # ang = -math.pi/2
        # x = vrep.simxSetJointPosition(self.clientID, joint, ang, vrep.simx_opmode_oneshot)

        # x, pos = vrep.simxGetJointPosition(self.clientID, joint, vrep.simx_opmode_streaming)

        return person
コード例 #17
0
    def _get_observation(self):     # get current observations s_t
        state = np.zeros(self.obs_dims)
        # state is [θ1, θ2, θ3, θ4, θ5, θ6, ω1, ω2, ω3, ω4, ω5, ω6,
        #           Base_x, Base_y, Base_z, Base_α, Base_β, Base_γ,
        #           Vx, Vy, Vz, dα, dβ, dγ]

        Joint_angle = [vrep.simxGetJointPosition(self.clientID, self.handles['joint'][i],
                                                 vrep.simx_opmode_blocking)[1]
                       for i in range(6)]      # retrieve the rotation angle as [θ1, θ2, θ3, θ4, θ5, θ6]
        state[0: 6] = Joint_angle

        Joint_velocity = [vrep.simxGetObjectFloatParameter(self.clientID, self.handles['joint'][i],
                                                           2012, vrep.simx_opmode_blocking)[1]
                          for i in range(6)]        # retrieve the joint velocity as [ω1, ω2, ω3, ω4, ω5, ω6]
        state[6: 12] = Joint_velocity

        Base_position = [vrep.simxGetObjectPosition(self.clientID, self.handles['base'][0],
                                                    -1, vrep.simx_opmode_blocking)[1]]
        state[12: 15] = Base_position[0]     # retrieve the position of Base as [Base_x, Base_y, Base_z]

        Base_pose = [vrep.simxGetObjectOrientation(self.clientID, self.handles['base'][0],
                                                   -1, vrep.simx_opmode_blocking)[1]]
        state[15: 18] = Base_pose[0]          # retrieve the orientation of Base as [Base_α, Base_β, Base_γ]

        _, Base_Vel, Base_Ang_Vel = vrep.simxGetObjectVelocity(self.clientID, self.handles['base'][0],
                                                               vrep.simx_opmode_blocking)
        state[18: 21] = Base_Vel        # retrieve the linear velocity of Base as [Vx, Vy, Vz]
        state[21: 24] = Base_Ang_Vel        # retrieve the angular velocity of Base as [dα, dβ, dγ]

        state = np.asarray(state)

        return state
コード例 #18
0
ファイル: hexapod.py プロジェクト: suhasjs/hops-framework
 def get_joint_vel(self):
     jvel = np.zeros((1, self.joint_count))
     for handle_idx in range(0, self.joint_count):
         _, jvel[0, handle_idx] = vrep.simxGetObjectFloatParameter(
             self.client_id, self.handles[handle_idx], 2012,
             vrep.simx_opmode_buffer)
     return jvel
コード例 #19
0
    def get_feedback(self):
        """ Return a dictionary of information needed by the controller.

        Returns the joint angles and joint velocities in [rad] and [rad/sec],
        respectively
        """

        for ii, joint_handle in enumerate(self.joint_handles):
            # get the joint angles
            _, self.q[ii] = vrep.simxGetJointPosition(
                self.clientID,
                joint_handle,
                vrep.simx_opmode_blocking)
            if _ != 0:
                raise Exception('Error retrieving joint angle, ' +
                                'return code ', _)

            # get the joint velocity
            _, self.dq[ii] = vrep.simxGetObjectFloatParameter(
                self.clientID,
                joint_handle,
                2012,  # ID for joint angular velocity
                vrep.simx_opmode_blocking)
            if _ != 0:
                raise Exception('Error retrieving joint velocity, ' +
                                'return code ', _)

        return {'q': self.q,
                'dq': self.dq}
コード例 #20
0
 def get_near_clip_plane(self, prec=None):
     """Retrieve near clipping plane."""
     if self._handle < 0:
         if self._handle == MISSING_HANDLE:
             raise RuntimeError(
                 "Could not retrieve near clipping plane of {}: missing "
                 "name or handle.".format(self._name))
         if self._handle == REMOVED_OBJ_HANDLE:
             raise RuntimeError(
                 "Could not retrieve near clipping plane of {}: object "
                 "removed.".format(self._name))
     client_id = self.client_id
     if client_id is None:
         raise ConnectionError(
             "Could not retrieve near clipping plane of {}: not connected "
             "to V-REP remote API server.".format(self._name))
     res, clip_plane = vrep.simxGetObjectFloatParameter(
         client_id, self._handle, vrep.sim_visionfloatparam_near_clipping,
         vrep.simx_opmode_blocking)
     if res != vrep.simx_return_ok:
         raise ServerError("Could not retrieve near clipping plane of {}."
                           "".format(self._name))
     if prec is not None:
         clip_plane = round(clip_plane, prec)  # near clipping plane may be
         # slightly imprecise due to
         # the use of single-precision
         # floating-point format by
         # V-REP
     return clip_plane
コード例 #21
0
    def retrieve_state(self):

        pos = np.zeros(1)
        vel = np.zeros(1)
        tip = np.zeros(3)

        _, pos[0] = vrep.simxGetJointPosition(self.clientID,
                                              self.jointHandles[0],
                                              vrep.simx_opmode_blocking)
        _, vel[0] = vrep.simxGetObjectFloatParameter(self.clientID,
                                                     self.jointHandles[0],
                                                     2012,
                                                     vrep.simx_opmode_blocking)
        _, tip_list = vrep.simxGetObjectPosition(self.clientID, self.tipHandle,
                                                 -1, vrep.simx_opmode_blocking)
        tip = np.asarray(tip_list)
        _, _1, jac_list, _2, _3 = vrep.simxCallScriptFunction(
            self.clientID, 'Jacobian_Script',
            vrep.sim_scripttype_childscript, 'jacobian_retriever', [], [], [],
            bytearray(), vrep.simx_opmode_blocking)
        jac = np.asarray(jac_list)
        jac = np.reshape(jac, (6, 1))
        jac = jac[0:3, :]

        return {1: pos, 2: vel, 3: tip, 5: jac}
コード例 #22
0
 def get_wheel_vel(self):
     """Returns the wheel velocities as a list of (left,right)."""
     vels = []
     for loc in self.wj_locs:
         _,vel = vrep.simxGetObjectFloatParameter(self.cid, self.wjs[loc], vrep.sim_jointfloatparam_velocity, self.op)
         vels.append(vel)
     return vels
コード例 #23
0
ファイル: hexapod.py プロジェクト: suhasjs/hops-framework
    def wait_till_stream(self):
        ret = vrep.simx_return_illegal_opmode_flag
        while ret != vrep.simx_return_ok:
            ret, _ = vrep.simxGetObjectOrientation(self.client_id,
                                                   self.body_handle, -1,
                                                   vrep.simx_opmode_buffer)

        ret = vrep.simx_return_illegal_opmode_flag
        while ret != vrep.simx_return_ok:
            ret, _ = vrep.simxGetObjectPosition(self.client_id,
                                                self.body_handle, -1,
                                                vrep.simx_opmode_buffer)

        for handle_idx in range(0, self.joint_count):
            ret = vrep.simx_return_illegal_opmode_flag
            while ret != vrep.simx_return_ok:
                ret, _ = vrep.simxGetObjectFloatParameter(
                    self.client_id, self.handles[handle_idx], 2012,
                    vrep.simx_opmode_buffer)
        for handle_idx in range(0, self.joint_count):
            ret = vrep.simx_return_illegal_opmode_flag
            while ret != vrep.simx_return_ok:
                ret, _ = vrep.simxGetJointPosition(self.client_id,
                                                   self.handles[handle_idx],
                                                   vrep.simx_opmode_buffer)
コード例 #24
0
ファイル: robot.py プロジェクト: anandsaha/stash
    def get_object_height(self, handle):
        time.sleep(self.sleep_sec_min)
        err, minval = vrep.simxGetObjectFloatParameter(self.clientID,
                                                       handle, vrep.sim_objfloatparam_modelbbox_min_z,
                                                       vrep.simx_opmode_blocking)

        if err != vrep.simx_return_ok:
            raise RuntimeError("get_object_height(): Failed to get parameters: sim_objfloatparam_modelbbox_min_z")

        err, maxval = vrep.simxGetObjectFloatParameter(self.clientID,
                                                       handle, vrep.sim_objfloatparam_modelbbox_max_z,
                                                       vrep.simx_opmode_blocking)
        if err != vrep.simx_return_ok:
            raise RuntimeError("get_object_height(): Failed to get parameters: sim_objfloatparam_modelbbox_max_z")

        return maxval - minval
コード例 #25
0
    def render_chair(self, table, position, offset):
        x, chair = vrep.simxLoadModel(self.clientID,
                                      self.models_path + "dining chair.ttm", 0,
                                      vrep.simx_opmode_blocking)
        x = vrep.simxSetObjectParent(self.clientID, chair, table, True,
                                     vrep.simx_opmode_blocking)
        offset_z = math.fabs(
            vrep.simxGetObjectFloatParameter(
                self.clientID, chair, vrep.sim_objfloatparam_modelbbox_min_z,
                vrep.simx_opmode_blocking)[1])
        vrep.simxSetObjectPosition(
            self.clientID, chair, -1,
            (position[0] + offset[0], position[1] + offset[1],
             position[2] + offset_z + offset[2]), vrep.simx_opmode_oneshot)
        vrep.simxSetObjectOrientation(self.clientID, chair, -1,
                                      (offset[3], offset[4], offset[5]),
                                      vrep.simx_opmode_oneshot)

        #returnCode, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
        # print (prop, prop + vrep.sim_modelproperty_not_respondable + vrep.sim_modelproperty_not_collidable)
        #returnCode2 = vrep.simxSetModelProperty(self.clientID, chair, prop  + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking)
        #returnCode3, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
        # print (returnCode, returnCode2, returnCode3, prop)
        # vrep.simxSetModelProperty(self.clientID,chair,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)

        return chair
コード例 #26
0
 def __init__(self, robot):
     self.id = vrep.simxStart('127.0.0.1', 19997, True, True, 1000, 5)
     if self.id == -1:
         exit('Connection to V-REP failed.')
     self.running = False
     self.joints = []
     for joint in robot['joints']:
         self.joints.append(
             vrep.simxGetObjectHandle(self.id, joint,
                                      vrep.simx_opmode_blocking)[1])
         vrep.simxGetJointPosition(self.id, self.joints[-1],
                                   vrep.simx_opmode_streaming)
         vrep.simxGetObjectFloatParameter(self.id, self.joints[-1],
                                          vrep.sim_jointfloatparam_velocity,
                                          vrep.simx_opmode_streaming)
     self.distances = {}
     self.dummies = {}
コード例 #27
0
    def __init__(self, client_id: int, handle: int, name: str):
        """Initialize sensor and get specific information

        Initialize
        """
        super().__init__(client_id, handle, name)
        # Assume that the sensor returns a square image
        __, self.res = vrep.simxGetObjectIntParameter(
            client_id, handle, vrep.sim_visionintparam_resolution_x,
            self.BLOCK)
        __, self.max_depth = vrep.simxGetObjectFloatParameter(
            client_id, handle, vrep.sim_visionfloatparam_far_clipping,
            self.BLOCK)
        __, angle_radians = vrep.simxGetObjectFloatParameter(
            client_id, handle, vrep.sim_visionfloatparam_perspective_angle,
            self.BLOCK)
        self.angle = round(degrees(angle_radians), 3)
コード例 #28
0
    def get_bbox(self) -> Tuple[np.ndarray, np.ndarray]:

        coords = tuple(
            vrep.simxGetObjectFloatParameter(self.client_id, self.handle, i,
                                             self.BLOCK)[1]
            for i in range(15, 21))
        return np.array(coords[:3], np.float32), \
               np.array(coords[3:], np.float32)
コード例 #29
0
    def object_properties(cls, com, handle):
        res, min_x = remote_api.simxGetObjectFloatParameter(
            com.api_id, handle, 21, remote_api.simx_opmode_oneshot_wait)
        assert res == 0
        min_x = 100 * min_x
        res, max_x = remote_api.simxGetObjectFloatParameter(
            com.api_id, handle, 24, remote_api.simx_opmode_oneshot_wait)
        assert res == 0
        res, min_y = remote_api.simxGetObjectFloatParameter(
            com.api_id, handle, 22, remote_api.simx_opmode_oneshot_wait)
        min_y = 100 * min_y
        assert res == 0
        res, max_y = remote_api.simxGetObjectFloatParameter(
            com.api_id, handle, 25, remote_api.simx_opmode_oneshot_wait)
        max_x = 100 * max_x
        assert res == 0
        res, min_z = remote_api.simxGetObjectFloatParameter(
            com.api_id, handle, 23, remote_api.simx_opmode_oneshot_wait)
        min_z = 100 * min_z
        assert res == 0
        res, max_z = remote_api.simxGetObjectFloatParameter(
            com.api_id, handle, 26, remote_api.simx_opmode_oneshot_wait)
        max_z = 100 * max_z
        assert res == 0
        res, mass = remote_api.simxGetObjectFloatParameter(
            com.api_id, handle, 3005, remote_api.simx_opmode_oneshot_wait)
        assert res == 0

        dims = (max_x - min_x, max_y - min_y, max_z - min_z)

        return dims, mass
コード例 #30
0
ファイル: ttt_files.py プロジェクト: humm/dovecot
    def object_properties(cls, com, handle):
        res, min_x = remote_api.simxGetObjectFloatParameter(com.api_id, handle,   21,
                                                            remote_api.simx_opmode_oneshot_wait)
        assert res == 0
        min_x = 100*min_x
        res, max_x = remote_api.simxGetObjectFloatParameter(com.api_id, handle,   24,
                                                            remote_api.simx_opmode_oneshot_wait)
        assert res == 0
        res, min_y = remote_api.simxGetObjectFloatParameter(com.api_id, handle,   22,
                                                            remote_api.simx_opmode_oneshot_wait)
        min_y = 100*min_y
        assert res == 0
        res, max_y = remote_api.simxGetObjectFloatParameter(com.api_id, handle,   25,
                                                            remote_api.simx_opmode_oneshot_wait)
        max_x = 100*max_x
        assert res == 0
        res, min_z = remote_api.simxGetObjectFloatParameter(com.api_id, handle,   23,
                                                            remote_api.simx_opmode_oneshot_wait)
        min_z = 100*min_z
        assert res == 0
        res, max_z = remote_api.simxGetObjectFloatParameter(com.api_id, handle,   26,
                                                            remote_api.simx_opmode_oneshot_wait)
        max_z = 100*max_z
        assert res == 0
        res, mass  = remote_api.simxGetObjectFloatParameter(com.api_id, handle, 3005,
                                                            remote_api.simx_opmode_oneshot_wait)
        assert res == 0

        dims = (max_x - min_x, max_y - min_y, max_z - min_z)

        return dims, mass
コード例 #31
0
ファイル: robots.py プロジェクト: bjkomer/nengo_vrep
    def handle_output(self):
        err, arm_ori = vrep.simxGetJointPosition(self.cid, self.arm_joint,
                                                   vrep.simx_opmode_oneshot)
        #2012 is the code for joint velocity
        err, arm_vel = vrep.simxGetObjectFloatParameter(self.cid,
                                                        self.arm_joint, 2012,
                                                        vrep.simx_opmode_oneshot)

        return [arm_ori, arm_vel]
コード例 #32
0
    def get_object_height(self, handle):
        time.sleep(self.sleep_sec_min)
        err, minval = vrep.simxGetObjectFloatParameter(
            self.clientID, handle, vrep.sim_objfloatparam_modelbbox_min_z,
            vrep.simx_opmode_blocking)

        if err != vrep.simx_return_ok:
            raise RuntimeError(
                "get_object_height(): Failed to get parameters: sim_objfloatparam_modelbbox_min_z"
            )

        err, maxval = vrep.simxGetObjectFloatParameter(
            self.clientID, handle, vrep.sim_objfloatparam_modelbbox_max_z,
            vrep.simx_opmode_blocking)
        if err != vrep.simx_return_ok:
            raise RuntimeError(
                "get_object_height(): Failed to get parameters: sim_objfloatparam_modelbbox_max_z"
            )

        return maxval - minval
コード例 #33
0
ファイル: mover.py プロジェクト: suhasjs/hops-framework
 def signal_values(self):
     ret = vrep.simx_return_illegal_opmode_flag
     while ret != vrep.simx_return_ok:
         ret, _ = vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1, vrep.simx_opmode_streaming)
     ret = vrep.simx_return_illegal_opmode_flag
     while ret != vrep.simx_return_ok:
         ret, _ = vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1, vrep.simx_opmode_streaming)
     ret = vrep.simx_return_illegal_opmode_flag
     while ret != vrep.simx_return_ok:
         ret, _ = vrep.simxGetJointPosition(self.client_id, self.left_joint, vrep.simx_opmode_streaming)
     ret = vrep.simx_return_illegal_opmode_flag
     while ret != vrep.simx_return_ok:
         ret, _ = vrep.simxGetJointPosition(self.client_id, self.right_joint, vrep.simx_opmode_streaming)
     ret = vrep.simx_return_illegal_opmode_flag
     while ret != vrep.simx_return_ok:
         ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.left_joint, 2012, vrep.simx_opmode_streaming)
     ret = vrep.simx_return_illegal_opmode_flag
     while ret != vrep.simx_return_ok:
         ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012,
                                                   vrep.simx_opmode_streaming)
コード例 #34
0
 def getRobotState(self):
     pos = np.zeros(len(self.joints))
     vel = np.zeros(len(self.joints))
     vrep.simxPauseCommunication(self.id, True)
     for i, joint in enumerate(self.joints):
         pos[i] = vrep.simxGetJointPosition(self.id, joint,
                                            vrep.simx_opmode_buffer)[1]
         vel[i] = vrep.simxGetObjectFloatParameter(
             self.id, joint, vrep.sim_jointfloatparam_velocity,
             vrep.simx_opmode_buffer)[1]
     vrep.simxPauseCommunication(self.id, False)
     return pos, vel
コード例 #35
0
ファイル: sample.py プロジェクト: arpitg1304/ObjectCapture
def callback(data):
    print('inside callback')
    data = data.data
    #print(data)
    #a,b,c = calc_parabola_vertex(data[0], data[1], data[2], data[3], data[4], data[5])
    a,b = calc_straight_line(data[0], data[1], data[4], data[5])
    print([a,b])
    z_req_vision= ((a*(x_req))+(b))/2
    # z_req_vision= (a*(x_req**2))+(b*x_req)+c
    print('z required from vision'+str(z_req_vision))
    print('x required from vision'+str(x_req))
    print('-'*8+'Checking if ball can be blocked'+'-'*8)
    transformation = np.array([[1,0,0,x_req*1000],[0,1,0,0],[0,0,1,z_req_vision*1000],[0,0,0,1]])
    desired_angles = robot.inverseKinematics(transformation)
    if desired_angles:
        print('-'*8+'Moving'+'-'*8)
        error = desired_angles-angles
        #error = np.array(error)
        error_collect.append(error)
        #error_collect.append(error.reshape(3,1))
        sum = np.sum(np.abs(error))
        Kp = [1000*(sum-np.abs(i))/sum for i in error]
        Kv = 100
        velocity_error = np.array([0 ,0 ,0])
        desired_velocity = np.copy(velocity_error)
        for i in range(3):
            while abs(error[i])>epsilon:
                vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot)
                # if error[i]>0.5:
                #     vrep.simxSetJointForce(clientID,joints[i],abs(Kp*0.5)+(Kv*velocity_error[i]),vrep.simx_opmode_oneshot)
                # else:#+(Kv*velocity_error[i])
                if abs(error[i])<epsilon:
                    vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
                    vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)

                vrep.simxSetJointForce(clientID,joints[i],abs(Kp[i]*(error[i]))+Kv*velocity_error[i],vrep.simx_opmode_oneshot)
                error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
                #print(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])

                if abs(error[i])<epsilon:
                    vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
                    vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)

                res,velocity = vrep.simxGetObjectFloatParameter(clientID,joints[i],2012,vrep.simx_opmode_blocking)
                velocity_error[i] = desired_velocity[i] - velocity
                #error_collect.append(error)

                #print(error)


        # while np.sum(np.abs(error))>epsilon:
        #     for i in range(6):
        #         if abs(error[i])<epsilon/6.:
        #             vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
        #             vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
        #         else:
        #             vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot)
        #             vrep.simxSetJointForce(clientID,joints[i],abs(Kp*(error[i])),vrep.simx_opmode_oneshot)
        #             error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
        a = []
        for i in range(3):
            vrep.simxSetJointTargetVelocity(clientID,joints[i],0,vrep.simx_opmode_oneshot)
            vrep.simxSetJointForce(clientID,joints[i],9999,vrep.simx_opmode_oneshot)
            a.append(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
        # print(a)
    else:
        for i in range(3):
            vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
            vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
    print('hi')
    sub_MTML.unregister()
コード例 #36
0
ファイル: quad_arm_2_ctrl.py プロジェクト: RajeshThevar/blog
                    vrep.simx_opmode_blocking)
            if _ !=0 : raise Exception()
            track_target.append(np.copy(target_xyz)) # store for plotting
            target_xyz = np.asarray(target_xyz)

            for ii,joint_handle in enumerate(joint_handles): 
                old_q = np.copy(q)
                # get the joint angles 
                _, q[ii] = vrep.simxGetJointPosition(clientID,
                        joint_handle,
                        vrep.simx_opmode_blocking)
                if _ !=0 : raise Exception()
                
                # get the joint velocity
                _, dq[ii] = vrep.simxGetObjectFloatParameter(clientID,
                        joint_handle,
                        2012, # parameter ID for angular velocity of the joint
                        vrep.simx_opmode_blocking)
                if _ !=0 : raise Exception()

            # update end-effector position
            L = np.array([0, .35]) # segment lengths associated with each joint 
            s = np.sin(q)
            c = np.cos(q)

            xyz = np.array([
                0, 
                L[1]*c[0],
                L[1]*s[0]])
            xyz += np.array([0, 0, 0.425])

            track_hand.append(np.copy(xyz)) # store for plotting
コード例 #37
0
ファイル: sensors.py プロジェクト: bjkomer/nengo_vrep
def joint_velocity(cid, handle):
    err, ret = vrep.simxGetObjectFloatParameter(cid, handle, 2012, vrep_mode)
    return [ret]