diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1c1dfc0 --- /dev/null +++ b/.gitignore @@ -0,0 +1,10 @@ +__pycache__ +*.py~ + +# *.so + +results +old + + +.idea diff --git a/AUTHORS b/AUTHORS new file mode 100644 index 0000000..c9ff069 --- /dev/null +++ b/AUTHORS @@ -0,0 +1,34 @@ ++--------------------------------------+---------------------------------------+ +| ABOUT | ++------------------------------------------------------------------------------+ + +This file contains the list of people involved in the development of +RL-ROBOT, which started at the +Machine Perception and Intelligent Robotics (MAPIR) laboratory +at the University of Malaga + +If you feel someone is missing, please fork and pull-request. +The following list is roughly sorted in reverse chronological order. + + ++--------------------------------------+---------------------------------------+ +| DEVELOPERS & CONTRIBUTORS | ++------------------------------------------------------------------------------+ + +* Angel Martinez-Tenor + angelmtenor@gmail.com + http://mapir.isa.uma.es/mapirwebsite/index.php/people/115-people/230-angel-martinez-tenor + main developer + + ++--------------------------------------+---------------------------------------+ +| OTHERS | ++------------------------------------------------------------------------------+ + +Bug reports and new feature suggestions, provided by users world-wide, +will usually be mentioned in the changelog. + +We kindly thank all of them for this valuable feedback. + + + --- END OF FILE --- \ No newline at end of file diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..960ccb5 --- /dev/null +++ b/LICENSE @@ -0,0 +1,19 @@ ++--------------------------------------+---------------------------------------+ +| LICENSE | ++------------------------------------------------------------------------------+ + +* RL-ROBOT is released under a GPLv3 license. Read license-GPLv3, + or if not present, . + +* For a closed-source version of RL-ROBOT + for commercial purposes, please contact the authors. + +* If you use RL-ROBOT in an academic work, + please cite the most relevant publication associated by visiting: + , or if any, please cite the + Machine Perception and Intelligent Robotics (MAPIR) + research group directly. + + + + --- END OF FILE --- \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..5f25afb --- /dev/null +++ b/README.md @@ -0,0 +1,31 @@ +# RL-ROBOT +This repository provides a Reinforcement Learning framework in Python from the Machine Perception and Intelligent Robotics research group [(MAPIR)](http://mapir.isa.uma.es). + +### Requirements +* Python 3 +* numpy +* matplotlib +* tkinter `sudo apt-get install python-tk` + + +### V-REP settings: +(Tested on V-REP_PRO_EDU_V3_3_2 64_bits Linux) + +1. Use default values of `remoteApiConnections.txt` + ~~~ + portIndex1_port = 19997 + portIndex1_debug = false + portIndex1_syncSimTrigger = true + ~~~ + +2. Activate threaded rendering (recommended): + `system/usrset.txt -> threadedRenderingDuringSimulation = 1` + +**Execute V-REP** (`./vrep.sh on linux`). `File -> Open Scene -> (open any scene for RL-ROS)` + +Recommended simulation settings for scenes in RL-ROS (already set in the provide ones): + +* Simulation step time: 50 ms (default) +* Real-Time Simulation: Enabled +* Multiplication factor: 3.00 (required CPU >= i3 3110m) + \ No newline at end of file diff --git a/__init__.py b/__init__.py new file mode 100644 index 0000000..2421832 --- /dev/null +++ b/__init__.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +-----------------------------------+-----------------------------------+ +# | RL-ROBOT | +# | | +# | Copyright (c) 2016, Individual contributors, see AUTHORS file. | +# | Machine Perception and Intelligent Robotics (MAPIR), | +# | University of Malaga. | +# | | +# | This program is free software: you can redistribute it and/or modify | +# | it under the terms of the GNU General Public License as published by | +# | the Free Software Foundation, either version 3 of the License, or | +# | (at your option) any later version. | +# | | +# | This program is distributed in the hope that it will be useful, | +# | but WITHOUT ANY WARRANTY; without even the implied warranty of | +# | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | +# | GNU General Public License for more details. | +# | | +# | You should have received a copy of the GNU General Public License | +# | along with this program. If not, see . | +# +-----------------------------------------------------------------------+ +""" RL-ROS init """ diff --git a/action_qbiasr.py b/action_qbiasr.py new file mode 100644 index 0000000..6a127c2 --- /dev/null +++ b/action_qbiasr.py @@ -0,0 +1,168 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Q-Biased Softmax Regression (QBIASR) v0.8.7 optimized """ + +import math +import random +from functools import reduce +from itertools import combinations + +import numpy as np + +import agent +import exp +import lp +import task + +DEFAULT_TEMPERATURE = exp.TEMPERATURE +temperature = DEFAULT_TEMPERATURE + +control_sequence = np.full(0, -1, dtype=np.int32) +rewards_sequence = np.full(0, -1, dtype=np.float32) + +mix = np.full(0, -1, dtype=np.int) +comb = np.full(0, -1, dtype=np.int) +initiated = False + + +def setup(): + """ Initializes QBIASR """ + global control_sequence, rewards_sequence, comb, mix, initiated + + # size_sequence = size of eli queue: n < log(threshold) / log(gamma*lambda) + threshold = 0.01 + size_sequence = int(math.log(threshold) / math.log(exp.GAMMA * exp.LAMBDA)) + + # size_sequence limits: [4, n_states/4] + lower_limit = 4 # Mandatory + upper_limit = int(task.n_states / 4) + if size_sequence > upper_limit: + size_sequence = upper_limit + if size_sequence < lower_limit: + size_sequence = lower_limit + control_sequence = np.full(size_sequence, -1, dtype=np.int32) + rewards_sequence = np.full(size_sequence, -1, dtype=np.float32) + + # Create mix[s], index[s], subrow[s] + n_inputs = task.n_inputs + n_states = task.n_states + + comb = np.array(list(combinations(range(n_inputs), n_inputs - 1)), + dtype=np.int16) + # comb = len(list(combinations(range(n_inputs), n_inputs - 1))) = n_inputs!! + mix = np.full([n_states, n_inputs, n_states], -1, dtype=np.int) + index = np.full(([n_states, n_inputs, n_states]), -1, dtype=np.int) + + for s in range(n_states): + ss = agent.unwrap_state(s) + + for i in range(ss.size): + j = ss[i] + n = agent.cont_VAR[i, j] + for k in range(n): + index[s, i, k] = agent.VAR[i, j, k] + + for idx, item in enumerate(comb): + matches = reduce(np.intersect1d, (index[s, item])) + mix[s, idx, 0:len(matches)] = matches + initiated = True + + +def custom_softmax(input_array, temp): + """ Softmax Boltzmann action selection given a vector and temperature """ + selected_action = -1 + # 1: Get the probabilities + _input_array_size = len(input_array) + _Pa = np.zeros(_input_array_size) + + for i in range(_input_array_size): + _Pa[i] = math.exp(input_array[i] / temp) + _Pa = np.divide(_Pa, sum(_Pa)) + + # 2: Select the action + ran = random.random() + accum = 0.0 + for i in range(_input_array_size): + accum = accum + _Pa[i] + if ran < accum: + selected_action = i + break + assert (selected_action > -1) + return selected_action + + +def select_biased_action(s): + """ Select an action 'a' given state 's' by QBIASR """ + assert initiated, " QBIASR not initiated! setup() must be called previously" + + # n_combinations = math.factorial(N_INPUTS)/(math.factorial(level)* + # math.factorial(N_INPUTS-level)) + n_actions = task.n_actions + q = lp.q + q_limit = lp.q_limit + bias_s = 0 + for c in range(len(comb)): + s_array = mix[s, c] + s_array = s_array[s_array >= 0] + subrow = np.zeros((len(s_array), n_actions)) + for idx, item in enumerate(s_array): + subrow[idx] = q[item] + # for k in range(len(s_array)): + # subrow[k] = q[s_array[k]] + aux = np.average(subrow, 0) + bias_s += aux / len(comb) + + low_reward_loop_evasion(s) + q_s_bias = q[s] + bias_s # q_s_bias = np.sum([q[s], bias_s], axis=0) + + # 2016_05_26: Temporal qs_bias row is normalized for softmax regression. + # Standard q_limit: 100 (e.g: Rmax=10, GAMMA=0.9) + q_s_bias *= 100.0 / q_limit + selected_action = custom_softmax(tuple(q_s_bias), temperature) + return selected_action + + +def low_reward_loop_evasion(s): + """ Increase the temperature if the agent is stuck in a sequence of states + with negative average reward """ + global temperature + global control_sequence + global rewards_sequence + + size_sequence = control_sequence.size + + # early steps of learning: + if lp.step < size_sequence: + temperature = DEFAULT_TEMPERATURE + return + + control_sequence = lp.sasr_step[lp.step - size_sequence:lp.step, 0] + # different state reached: + if s not in control_sequence: + temperature = DEFAULT_TEMPERATURE + return + + # not enough repeated states: + unique_sequence = np.unique(control_sequence) + loop_rate = control_sequence.size / unique_sequence.size + if loop_rate <= 2: + temperature = DEFAULT_TEMPERATURE + return + + # average reward positive: + rewards_sequence = lp.sasr_step[lp.step - size_sequence:lp.step, 3] + if np.average(rewards_sequence) > 0: + temperature = DEFAULT_TEMPERATURE + return + + # low reward loop detected. Evasion: + temperature += 0.25 * loop_rate + if temperature > 50: + temperature = 50 + # print(" Local maximum detected at: ",str(s_unique)) + # print(" Temperature changed to: %0.2f" %temperature) + return diff --git a/action_selection.py b/action_selection.py new file mode 100755 index 0000000..99bfb47 --- /dev/null +++ b/action_selection.py @@ -0,0 +1,137 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Exploration-exploitation strategy """ + +import math +import random +import sys + +import numpy as np + +import exp +import lp +import task + +if exp.ACTION_STRATEGY == "QBIASR": + import action_qbiasr + +EPSILON = 0.3 # e-greedy +LEAST_EXPLORED = 0.3 # Probability of choose the least explored action +TEMPERATURE = exp.TEMPERATURE # Temperature Parameter for Softmax Boltzmann +initiated = False + + +def setup(): + """ Initialize QBIASR if needed """ + global initiated + if exp.ACTION_STRATEGY == "QBIASR": + action_qbiasr.setup() + initiated = True + + +def execute(s): + """ From state s select an action a """ + + if exp.TEACH_THE_ROBOT: + print("Warning: Controlling the robot for teaching not implemented") + pass + # selected_action = task.key2action() ToDo: re-implement teaching + # return selected_action + + elif exp.TEACHING_PROCESS: + if lp.step >= exp.TEACHING_STEPS: + exp.TEACHING_PROCESS = False + else: + return exp.TAUGHT_SASR[lp.step, 1] + + # if EXP.EXECUTE_GIVEN_POLICY: + # selected_action = TASK.OPTIMAL_POLICY[s] + + if exp.ACTION_STRATEGY == "exploit": + selected_action = exploit_policy(s) + + elif exp.ACTION_STRATEGY == "random": + selected_action = random_action() + + elif exp.ACTION_STRATEGY == "eGreedy": + selected_action = egreedy(s, EPSILON) + + # elif EXP.ACTION_STRATEGY == "E-GREEDY_IMPROVE_LEAST_EXPLORED": + # selected_action = egreedyLeastExplored(s, EPSILON, LEAST_EXPLORED)""" + + elif exp.ACTION_STRATEGY == "softmax": + selected_action = softmax(s) + + elif exp.ACTION_STRATEGY == "QBIASR": # novel technique + selected_action = action_qbiasr.select_biased_action(s) + + else: + sys.exit("ERROR: WRONG ACTION STRATEGY: " + exp.ACTION_STRATEGY) + return selected_action + + +def exploit_policy(s): + """ Exploit the action a given an state s according to the Policy """ + selected_action = lp.policy[s] + return selected_action + + +def random_action(): + """ Select a random action a (uniform distribution) """ + # random.seed() + selected_action = random.randint(0, task.n_actions - 1) + return selected_action + + +def egreedy(s, e): # if e = 0.3_: 30% exploration + """ Select an action a given a state s based on egreedy exploration """ + # random.seed() + if random.random() < e: + selected_action = random_action() + else: + selected_action = exploit_policy(s) + return selected_action + + +def egreedy_least_explored(s, e, least): + """ Select an action a given a state s based on egreedy exploration + improving the probability of selecting the least explored action """ + # random.seed() + if random.random() < e: + if random.random() < least: + selected_action = 0 + for i in range(task.n_actions): + if lp.q_count[s, i] < lp.q_count[s, selected_action]: + # exploration holds the number of times that the cells + # of Q[s, a] have been explored + selected_action = i + else: + selected_action = random_action() + else: + selected_action = exploit_policy(s) + return selected_action + + +def softmax(s): + """ Select an action a given a state s based on Boltzmann exploration """ + selected_action = -1 + # 1: Get the probabilities + pa = np.zeros(task.n_actions) + for i in range(task.n_actions): + pa[i] = math.exp(lp.q[s, i] / TEMPERATURE) + pa = np.divide(pa, sum(pa)) + + # 2: Select the action + # random.seed() + ran = random.random() + accum = 0.0 + for i in range(task.n_actions): + accum = accum + pa[i] + if ran < accum: + selected_action = i + break + return selected_action diff --git a/agent.py b/agent.py new file mode 100644 index 0000000..34bd8bd --- /dev/null +++ b/agent.py @@ -0,0 +1,286 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Abstract agent """ + +import numpy as np + +import action_selection +import exp +import robot +import task + +n_inputs = int +in_values = [None] +in_sizes = [int] +n_outputs = int +out_values = [None] +out_sizes = [int] + +n_states = int +n_actions = int + +Vs = np.empty(0) +Va = np.empty(0) +VAR = np.empty(0) +cont_VAR = np.empty(0) +initiated = False + + +def setup_task(): + """ Task setup will be performed in the agent """ + global n_inputs, in_values, n_outputs, out_values, Vs, Va, VAR, cont_VAR + global in_sizes, out_sizes, n_states, n_actions, initiated + + inputvar = task.INPUT_VARIABLES + outputvar = task.OUTPUT_VARIABLES + + n_inputs = len(inputvar) + in_values = [None] * n_inputs + in_names = [None] * n_inputs + in_sizes = [int] * n_inputs + i = 0 + for key, value in inputvar.items(): + in_names[i] = key + in_values[i] = value + in_sizes[i] = len(value) + i += 1 + n_states = int(np.prod(in_sizes)) + input_data = np.zeros(n_inputs) + + n_outputs = len(outputvar) + out_values = [None] * n_outputs + out_names = [None] * n_outputs + out_sizes = [int] * n_outputs + i = 0 + for key, value in outputvar.items(): + out_names[i] = key + out_values[i] = value + out_sizes[i] = len(value) + i += 1 + n_actions = int(np.prod(out_sizes)) + output_data = np.zeros(n_outputs) + + # in_values = np.array(in_values) + # in_sizes = np.array(in_sizes) + # input_data = np.array(input_data) + # out_values = np.array(out_values) + # out_sizes = np.array(out_sizes) + # output_data = np.array(output_data) + + task.n_inputs = n_inputs + task.in_values = in_values + task.in_names = in_names + task.in_sizes = in_sizes + task.n_states = n_states + task.in_data = input_data + + task.n_outputs = n_outputs + task.out_values = out_values + task.out_names = out_names + task.out_sizes = out_sizes + task.n_actions = n_actions + task.out_data = output_data + + print("Task setup: {} \t States: {} \t Actions {} ".format( + task.NAME, n_states, n_actions)) + + +def setup(): + """ Create the variables needed for this module """ + global Vs, Va, VAR, cont_VAR, initiated + + if initiated: + return + + robot.setup(task.AGENT_ELEMENTS, task.ENV_ELEMENTS) + + Vs = 0 + Va = 0 + VAR = np.full((n_inputs, int(max(in_sizes)), n_states), -1, + dtype=np.int) + cont_VAR = np.full((n_inputs, int(max(in_sizes))), 0, dtype=np.int) + + generate_vs() + generate_va() + generate_var() + + action_selection.setup() + + initiated = True + return + + +def observe_state(): + """ Returns the reached state s' from robot """ + assert initiated, "agent not initiated! setup() must be previously executed" + + unwrapped_s = np.zeros(n_inputs) + + # Special cases + if exp.TEACHING_PROCESS: # Observed states are already given + from lp import step + return exp.TAUGHT_SASR[step, 2] + elif exp.LEARN_FROM_MODEL: + from lp import s, a + import model + return model.get_sp(s, a) # return reached state s' + + robot.update() + + input_data = task.get_input_data() + + for i in range(n_inputs): + aux = np.digitize(input_data[i], in_values[i], right=True) + unwrapped_s[i] = int(np.clip(aux - 1, 0, in_sizes[i] - 1)) + # print("var: "+str(i)) + # print(input_data[i]) + # print(INPUT[i]) + # print(aux) + # print(unwrapped_s[i]) + + state = wrap_state(unwrapped_s) + + assert (0 <= state < n_states), ("Wrong state: ", str(state)) + return state + + +def select_action(s): + """ Return action a by calling the action selection strategy """ + a = action_selection.execute(s) + return a + + +# ------------------------------------------------------------------------------ +def execute_action(a): + """ Execute action in robot """ + # Special cases + if exp.LEARN_FROM_MODEL: + return + elif exp.TEACHING_PROCESS and exp.SKIP_VIEW_TEACHING: + return + assert (0 <= a < n_actions), ("Wrong action: ", str(a)) + + unwrapped_a = unwrap_action(a) + actuator = np.zeros(n_outputs) + for i in range(n_outputs): + actuator[i] = Va[i, unwrapped_a[i]] + + task.execute_action(actuator) + return + + +# ------------------------------------------------------------------------------ +def obtain_reward(s, a, sp): + """ Return the reward obtained """ + # Special cases + if exp.TEACHING_PROCESS: + from lp import step + if step >= exp.TEACHING_STEPS: + exp.TEACHING_PROCESS = False # End of teaching + else: + return exp.TAUGHT_SASR[step, 3] + if exp.LEARN_FROM_MODEL: + # from lp import s, a, sp + import model + return model.get_r(s, a, sp) + + r = task.get_reward() # (s,a, sp) arguments not needed here + return r + + +# ------------------------------------------------------------------------------ +def wrap_state(unw_s): + """ Compose the global state from an array of substates """ + s = unw_s[0] + for i in range(1, n_inputs): + pro = 1 + for j in range(0, i): + pro *= in_sizes[j] + s += pro * unw_s[i] + assert (0 <= s < n_states), ("Wrong state: ", str(s)) + return int(s) + + +# ------------------------------------------------------------------------------ +def unwrap_state(s): + """ Return the array of substates from the global state s """ + assert (0 <= s < n_states), ("Wrong state: ", str(s)) + unwrapped_s = np.zeros(n_inputs, dtype=np.int) + aux = s + for i in range(n_inputs - 1): + unwrapped_s[i] = aux % in_sizes[i] + aux = int(aux / in_sizes[i]) + unwrapped_s[n_inputs - 1] = aux + return unwrapped_s + + +# ------------------------------------------------------------------------------ +def wrap_action(unw_a): + """ Compose the global action from an array of subactions """ + a = unw_a[0] + for i in range(1, n_outputs): + pro = 1 + for j in range(0, i): + pro *= out_sizes[j] + a += pro * unw_a[i] + assert (0 <= a < n_actions), ("Wrong action: ", str(a)) + return int(a) + + +# ------------------------------------------------------------------------------ +def unwrap_action(a): + """ Return the array of subactions from the global action a """ + assert (0 <= a < n_actions), ("Wrong action: ", str(a)) + unwrapped_a = np.zeros(n_outputs, dtype=np.int) + aux = a + for i in range(n_outputs - 1): + unwrapped_a[i] = aux % out_sizes[i] + aux = int(aux / out_sizes[i]) + unwrapped_a[n_outputs - 1] = aux + return unwrapped_a + + +# ------------------------------------------------------------------------------ +def generate_vs(): + """ Generate array of substates """ + global Vs + Vs = np.zeros([n_inputs, int(max(in_sizes))]) + for i in range(n_inputs): + for idx, item in enumerate(in_values[i]): + Vs[i, idx] = item + + +# ------------------------------------------------------------------------------ +def generate_va(): + """ Generate array of subactions """ + global Va + Va = np.zeros([n_outputs, int(max(out_sizes))]) + + for i in range(n_outputs): + for idx, item in enumerate(out_values[i]): + Va[i, idx] = item + + +# ------------------------------------------------------------------------------ +def generate_var(): + """ Generate Variable Matrix (input, input_value, count) -> state """ + global VAR, cont_VAR + VAR = np.full((n_inputs, int(max(in_sizes)), n_states), -1, + dtype=np.int) + cont_VAR = np.full((n_inputs, int(max(in_sizes))), 0, dtype=np.int) + + for s in range(n_states): + ss = unwrap_state(s) + for i in range(ss.size): + # print ss + # print ss.size + # print i + j = ss[i] + k = cont_VAR[i, j] + VAR[i, j, k] = s + cont_VAR[i, j] += 1 + return diff --git a/algorithm_q.py b/algorithm_q.py new file mode 100755 index 0000000..77d50b8 --- /dev/null +++ b/algorithm_q.py @@ -0,0 +1,58 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Q-learning algorithm """ + +import time + +import numpy as np + +import agent +import exp +import lp + +ID = "Q" +CAPTION = "Q-learning" + + +def setup(): + """ Setup algorithm """ + pass # no needed here + + +def execute(): + """ Execute the learning algorithm """ + s = lp.s + alpha = lp.alpha + q = lp.q + v = lp.v + policy = lp.policy + + a = ap = agent.select_action(s) + + agent.execute_action(a) + time.sleep(lp.step_time) + + sp = agent.observe_state() + r = agent.obtain_reward(s, a, sp) + + # update Q: + delta = r + exp.GAMMA * v[sp] - q[s, a] # TD error (Q-learning) + q[s, a] = q[s, a] + alpha * delta # Update rule + + # update V and Policy: + v[s] = np.max(q[s]) + policy[s] = np.argmax(q[s]) + lp.s = s + lp.a = a + lp.sp = sp + lp.ap = ap + lp.r = r + lp.alpha = alpha + lp.q = q + lp.v = v + lp.policy = policy + lp.delta = delta diff --git a/algorithm_s.py b/algorithm_s.py new file mode 100755 index 0000000..ac6520c --- /dev/null +++ b/algorithm_s.py @@ -0,0 +1,60 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" SARSA algorithm """ + +import time + +import numpy as np + +import agent +import exp +import lp + +ID = "S" +CAPTION = "SARSA" + + +def setup(): + """ Setup algorithm """ + pass # no needed here + + +def execute(): + """ Execute the learning algorithm """ + s = lp.s + a = lp.a + alpha = lp.alpha + q = lp.q + v = lp.v + policy = lp.policy + + agent.execute_action(a) + time.sleep(lp.step_time) + + sp = agent.observe_state() + r = agent.obtain_reward(s, a, sp) + + ap = agent.select_action(sp) + + # update Q + delta = r + exp.GAMMA * q[sp, ap] - q[s, a] # TD error (SARSA) + q[s, a] = q[s, a] + alpha * delta # update rule + + # Update V and Policy + v[s] = np.max(q[s]) + policy[s] = np.argmax(q[s]) + + lp.s = s + lp.a = a + lp.sp = sp + lp.ap = ap + lp.r = r + lp.alpha = alpha + lp.q = q + lp.v = v + lp.policy = policy + lp.delta = delta diff --git a/algorithm_sl.py b/algorithm_sl.py new file mode 100755 index 0000000..7ae3f31 --- /dev/null +++ b/algorithm_sl.py @@ -0,0 +1,98 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" SARSA(lambda) algorithm """ + +import math +import time +from collections import deque + +import numpy as np + +import agent +import exp +import lp +import task + +ID = "SL" +CAPTION = "SARSA Lambda" + +eligibility = None +eli_queue = deque([]) +initiated = False + + +def setup(): + """ Initializes global variables """ + global eligibility, eli_queue, initiated + + eligibility = np.zeros((task.n_states, task.n_actions), dtype=np.float64) + + # Reduced ET: + # size of queue: n < log(threshold) / log(gamma*lambda) + threshold = 0.01 + eli_size = int(math.log(threshold) / math.log(exp.GAMMA * exp.LAMBDA)) + # a FIFO queue will hold the states to update (eligibility >= 0.01) + eli_queue = deque([], eli_size) + + initiated = True + + +def execute(): + """ Execute the learning algorithm """ + + global eligibility + + assert initiated, " SL not initiated! setup() must be previously called" + + s = lp.s + a = lp.a + alpha = lp.alpha + q = lp.q + v = lp.v + policy = lp.policy + + # Specific Learning Algorithm + agent.execute_action(a) + time.sleep(lp.step_time) + sp = agent.observe_state() + r = agent.obtain_reward(s, a, sp) + + ap = agent.select_action(sp) # Exploration strategy + + delta = r + exp.GAMMA * q[sp, ap] - q[s, a] # TD error + + eligibility[s, a] = 1.0 # replace trace + + if eli_queue.count(s) > 0: + eli_queue.remove(s) + assert eli_queue.count(s) == 0, ("duplicated states found in ET: ", str(s)) + eli_queue.appendleft(s) + + # only the states in eli_queue are updated: + for i in eli_queue: # no all states updated, just those in eli_queue + # replace eli_queue by range(task.n_states) for non-reduced ET + for j in range(task.n_actions): + if eligibility[i, j] > 0.01: + q[i, j] = q[i, j] + alpha * delta * eligibility[i, j] + eligibility[i, j] *= exp.GAMMA * exp.LAMBDA + else: + eligibility[i, j] = 0 + + # update v and policy + v[i] = np.max(q[i]) + policy[i] = np.argmax(q[i]) + + lp.s = s + lp.a = a + lp.sp = sp + lp.ap = ap + lp.r = r + lp.alpha = alpha + lp.q = q + lp.v = v + lp.policy = policy + lp.delta = delta diff --git a/algorithm_tosl.py b/algorithm_tosl.py new file mode 100644 index 0000000..1184deb --- /dev/null +++ b/algorithm_tosl.py @@ -0,0 +1,104 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" True Online SARSA(lambda) (TOSL) algorithm """ + +import math +import time +from collections import deque + +import numpy as np + +import agent +import exp +import lp +import task + +ID = "TOSL" +CAPTION = "True Online Sarsa Lambda" + +eligibility = None +q_old = 0 +eli_queue = deque([]) +initiated = False + + +def setup(): + """ Initializes global variables """ + global eligibility, q_old, eli_queue, initiated + + eligibility = np.zeros((task.n_states, task.n_actions), dtype=np.float64) + q_old = 0 + + # Reduced ET: + # size of queue: n < log(threshold) / log(gamma*lambda) + threshold = 0.01 + eli_size = int(math.log(threshold) / math.log(exp.GAMMA * exp.LAMBDA)) + # a FIFO queue will hold the states to update (eligibility >= 0.01) + eli_queue = deque([], eli_size) + initiated = True + + +def execute(): + """ Execute Learning Algorithm """ + + global eligibility, q_old + + assert initiated, "TOSL not initiated! setup() must be previously called" + + s = lp.s + a = lp.a + alpha = lp.alpha + q = lp.q + v = lp.v + policy = lp.policy + + # Specific Learning Algorithm + agent.execute_action(a) + time.sleep(lp.step_time) + # robot.stop() + # time.sleep(TASK.STEP_TIME/2) + sp = agent.observe_state() + r = agent.obtain_reward(s, a, sp) + + ap = agent.select_action(sp) # Exploration strategy + + diff_q = q[s, a] - q_old + q_old = q[sp, ap] + + delta = r + exp.GAMMA * q[sp, ap] - q[s, a] # TD error + + eligibility[s, a] = (1.0 - alpha) * eligibility[s, a] + 1 + + if eli_queue.count(s) > 0: + eli_queue.remove(s) + assert eli_queue.count(s) == 0, ("duplicated states found in ET ", str(s)) + eli_queue.appendleft(s) + + for i in eli_queue: # no all states updated, just those in eli_queue + # replace eli_queue by range(task.n_states) for non-reduced ET + for j in range(task.n_actions): + if eligibility[i, j] > 0.01: + q[i, j] = q[i, j] + alpha * (delta + diff_q) * eligibility[i, j] + eligibility[i, j] *= exp.GAMMA * exp.LAMBDA + else: + eligibility[i, j] = 0 + + if i == s and j == a: + q[i, j] = q[i, j] - alpha * diff_q + + # update v and policy + v[i] = np.max(q[i]) + policy[i] = np.argmax(q[i]) + + lp.s, lp.a = s, a + lp.sp, lp.ap = sp, ap + lp.r = r + lp.alpha = alpha + lp.q = q + lp.v = v + lp.policy = policy + lp.delta = delta diff --git a/example.py b/example.py new file mode 100644 index 0000000..2b2e5da --- /dev/null +++ b/example.py @@ -0,0 +1,32 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" RL-ROBOT usage example """ + +import exp +import rlrobot + +exp.ENVIRONMENT_TYPE = "MODEL" +exp.TASK_ID = "wander_1k" +exp.FILE_MODEL = exp.TASK_ID + "_model" +exp.N_EPISODES_MODEL = 1 + +exp.ALGORITHM = "TOSL" +exp.ACTION_STRATEGY = "QBIASR" + +exp.N_REPETITIONS = 1000 +exp.N_EPISODES = 1 +exp.N_STEPS = 1 * 10 * 60 + +exp.SUFFIX = "non ET reduced" +exp.DISPLAY_STEP = 0 + +rlrobot.run() + +# ---------------------- + +# exp.ACTION_STRATEGY = "softmax" +# rlrobot.run() diff --git a/exp.py b/exp.py new file mode 100755 index 0000000..434b884 --- /dev/null +++ b/exp.py @@ -0,0 +1,105 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Experiment parameters """ + +import time + +import numpy as np + +# Basic parameters ------------------------------------------------------------- +TASK_ID = "wander_1Ks" # task filename from tasks folder +ENVIRONMENT_TYPE = "MODEL" # available: "MODEL", "VREP", "ROS" +SPEED_RATE = 3.0 # Recommended: REAL ROBOT: 1.0 (x1), VREP: 3.0 (x3) + +N_REPETITIONS = 100 # Number of repetitions of the experiment +N_EPISODES = 1 # >1 for episodic experiments: Uses arrays from previous epi +N_STEPS = 1 * 60 * 60 # 1 step ~ 1 second (Sets LE.N_STEPS) + +FILE_MODEL = TASK_ID + "_model" # MODEL environment only +N_EPISODES_MODEL = 1 # MODEL environment only +CONTINUE_PREVIOUS_EXP = False +PREVIOUS_EXP_FILE = "" + +DISPLAY_STEP = 0 # Policy will be printed each DISPLAY_STEP + +# Learning parameters ---------------------------------------------------------- +ALGORITHM = "TOSL" # "TOSL": true online SARSA lambda, "SL": SARSA lambda, +# "S": SARSA, Q: "Q-learning +ACTION_STRATEGY = "QBIASR" # "QBIASR"; Q-biased softmax regression, +# "softmax": softmax regression, "eGreedy", "random", "exploit" +ALPHA = 0.1 +GAMMA = 0.9 +LAMBDA = 0.9 +TEMPERATURE = 1 +SUFFIX = "" # str(N_STEPS) + "steps_" + str(N_EPISODES) +"epi" + +# Extra ----------------------------------------------------------------------- +EXPORT_SASR_step = False # Needed to create a Markovian model for further +# simulation ToDo: rlrobot.py uses large array sasr_step. To Delete + +USE_T_SAS_AND_R_SAS = False # Needed to create a Markovian model to be used +# within the learning process (Model-based RL). Not used so far. ToDo + +# Teaching +LEARN_FROM_TEACHING = False +TEACHING_STEPS = 199 +TEACHING_FILE = "" +SKIP_VIEW_TEACHING = True + +TEACH_THE_ROBOT = False + +# --------------------- Do not modify below ------------------------------------ + +LEARN_FROM_MODEL = False +TEACHING_PROCESS = False +EPISODIC = False +TAUGHT_SASR = False + + +def check(): + """ Check experiment parameters """ + global N_STEPS, LEARN_FROM_MODEL, TEACHING_STEPS, TEACHING_PROCESS + global EPISODIC, DISPLAY_STEP, TAUGHT_SASR + + LEARN_FROM_MODEL = False + EPISODIC = False + + if N_EPISODES > 1: + EPISODIC = True + + N_STEPS = int(N_STEPS) + + if ENVIRONMENT_TYPE == "MODEL": + LEARN_FROM_MODEL = True + + assert (not (TEACH_THE_ROBOT and LEARN_FROM_TEACHING)), ( + "\n Teaching and Learning at the same time \n") + + if LEARN_FROM_TEACHING: + TEACHING_PROCESS = True + # NAME = NAME + "_teaching" + TAUGHT_SASR = np.load(TEACHING_FILE) + aux = np.size(TAUGHT_SASR[:, 0]) + if TEACHING_STEPS > aux: + TEACHING_STEPS = aux + print("\nWarning, teaching steps reduced to " + str(aux)) + + if TEACH_THE_ROBOT: + print("\nTEACHING IN 3 SECONDS (keys W,A,S,D) \n") + time.sleep(3) + print("\nGO !!! \n") + + if ENVIRONMENT_TYPE == "VREP" and SPEED_RATE == 1: + print("\n\n\n\n WARNING: VREP WITH SPEED_RATE = 1 \n\n") + time.sleep(10) + + if ENVIRONMENT_TYPE == "ROS" and SPEED_RATE > 1: + print("\n\n\n\n WARNING: ROS WITH SPEED_RATE: ", SPEED_RATE, "\n\n") + time.sleep(10) + + if not DISPLAY_STEP: + DISPLAY_STEP = int(1e6) diff --git a/license-GPLv3 b/license-GPLv3 new file mode 100644 index 0000000..9cecc1d --- /dev/null +++ b/license-GPLv3 @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + {one line to give the program's name and a brief idea of what it does.} + Copyright (C) {year} {name of author} + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + {project} Copyright (C) {year} {fullname} + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/lp.py b/lp.py new file mode 100755 index 0000000..b5349c8 --- /dev/null +++ b/lp.py @@ -0,0 +1,179 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Learning process """ + +import time + +import numpy as np + +import agent +import exp +import show +import task + +learning_module = "algorithm_" + exp.ALGORITHM.lower() +learning_algorithm = __import__(learning_module) +np.set_printoptions(precision=2) + +step_time = 0 +step = -1 # Current learning step +a = -1 # Current action +s = -1 # current state +sp = -1 # state reached (s') +s0 = -1 # initial state +ap = -1 # next action selected (a') +r = 0 # current reward obtained (R) + +delta = 0 # Temporal Difference (TD) Error +q = None # Q-matrix +v = None # Value function +policy = None # Current Policy +q_count = None +alpha = 0 + +elapsed_time = 0 # seconds +actual_step_time = 0 # seconds: simulation time / actual_time +final_average_reward = 0 # Resulting average R at last step + +ave_v_step = None # Average V per step (for plotting) +ave_r_step = None # Average R obtained per step +sasr_step = None # History of (s,a,s',R) per step + +q_limit = 0 + +if exp.USE_T_SAS_AND_R_SAS: + t_sas = np.zeros((task.n_states, task.n_actions, task.n_states)) + # Number of steps in which from s, executing a, resulted sp + r_sas = np.zeros((task.n_states, task.n_actions, task.n_states)) + # Sum of Rewards for s,a,sp +initiated = False + +initial_step_time = step_time # auxiliary + + +def setup(): + """ Create module variables """ + global step_time, step, s, sp, a, ap, r, alpha, delta, q, v, policy, q_count + global t_sas, r_sas, elapsed_time, actual_step_time + global final_average_reward, ave_v_step, ave_r_step, sasr_step, q_limit, s0 + global initiated, initial_step_time + + agent.setup() + + step_time = task.STEP_TIME / exp.SPEED_RATE + initial_step_time = step_time + # only used in case we want to modify step_time from a task module + # to speed up the experiment when the robot reaches the goal + step = 0 + # Get initial state: + if exp.LEARN_FROM_MODEL: + import model + s = int(model.s0) + else: + s = agent.observe_state() + s0 = s + + sp = -1 + a = task.INITIAL_POLICY + ap = -1 + r = 0 + alpha = exp.ALPHA + delta = 0 + + q = np.zeros((task.n_states, task.n_actions), dtype=np.float64) + v = np.zeros(task.n_states, dtype=np.float64) + policy = np.full(task.n_states, task.INITIAL_POLICY, dtype=np.uint32) + q_count = np.zeros((task.n_states, task.n_actions), dtype=np.uint64) + + if exp.USE_T_SAS_AND_R_SAS: + t_sas = np.zeros((task.n_states, task.n_actions, task.n_states)) + r_sas = np.zeros((task.n_states, task.n_actions, task.n_states)) + + elapsed_time = 0 + actual_step_time = 0 + final_average_reward = 0 + + ave_v_step = np.zeros(exp.N_STEPS) + ave_r_step = np.zeros(exp.N_STEPS) + sasr_step = np.zeros((exp.N_STEPS, 4)) + + learning_algorithm.setup() + + q_limit = round(max(task.REWARDS) / (1 - exp.GAMMA)) + # q_limit = max(TASK.REWARDS)/(1-EXP.GAMMA) + + if q_limit != 100: + print("q_limit = ", str(q_limit), + ". Softmax regression will be normalized as q_limit = 100") + time.sleep(2) + + initiated = True + return + + +def run(): + """ Execute the learning Process""" + global step, s, sp, a, ap, r, alpha + global q, v, policy, q_count + global t_sas, r_sas + global ave_r_step, sasr_step + global elapsed_time, actual_step_time + global final_average_reward + + assert initiated, ("learning process not initiated! setup() " + "must be previously called") + mark = time.time() + + # Start learning process: ----------------------------- + for step in range(0, exp.N_STEPS): + + if step > 0 and step % exp.DISPLAY_STEP == 0: + print("STEP: ", step) + + if task.goal_reached: # rest of the process filled with highest reward + sasr_step[step, 3] = r + ave_r_step[step] = np.average(sasr_step[0: step, 3]) + continue + + # Execute the selected learning algorithm. Also change lp variables + learning_algorithm.execute() + + # update analysis and model arrays + q_count[s, a] += 1 + sasr_step[step, 0] = s + sasr_step[step, 1] = a + sasr_step[step, 2] = sp + sasr_step[step, 3] = r + ave_v_step[step] = np.mean(v) + if step == 0: + ave_r_step[step] = sasr_step[step, 3] + else: + ave_r_step[step] = np.average(sasr_step[0: step, 3]) + + # Display information + if step > 0 and step % exp.DISPLAY_STEP == 0: + print("s:", s, " a:", a, " sp:", sp, " R: %0.2f" % r) + print("Average Reward: %0.2f" % ave_r_step[step], "\n") + if exp.TEACHING_PROCESS: + print("--- Teaching ---") + + if exp.USE_T_SAS_AND_R_SAS: + t_sas[s, a, sp] += 1 + r_sas[s, a, sp] += r + + # Update state + s = sp + a = ap + + # End of learning process ---------------------- + + final_average_reward = ave_r_step[step] + elapsed_time = (time.time() - mark) + actual_step_time = elapsed_time / (step + 1) + + show.process_summary() + return diff --git a/model.py b/model.py new file mode 100644 index 0000000..53b3821 --- /dev/null +++ b/model.py @@ -0,0 +1,129 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Simulation from Markovian model """ + +import os.path +import random + +import numpy as np + +import task + +# transition matrix of the model: +t = np.empty(0) +# Reward matrix of the model: +r = np.empty(0) +# Initial State (can be obtained directly from a 'SASR_step' datafile): +s0 = -1 +freq_t = np.empty(0) +freq_r = np.empty(0) + + +def generate_t_and_r(datafile_model, n_episodes_model=1): + """ generate Transition and Reward functions from a 'SASR_step' datafile """ + global t, r, s0, freq_t, freq_r + t = np.zeros((task.n_states, task.n_actions, task.n_states), + dtype=np.float16) + r = np.zeros((task.n_states, task.n_actions, task.n_states, + task.REWARDS.size), dtype=np.float16) + + freq_t = np.zeros((task.n_states, task.n_actions, task.n_states), + dtype=np.uint32) + freq_r = np.zeros((task.n_states, task.n_actions, task.n_states, + task.REWARDS.size), dtype=np.uint32) + print("Generating T and R. Please wait ...") + + for epi in range(n_episodes_model): + filename = datafile_model + "_SASR_step" + if n_episodes_model > 1: + filename = datafile_model + "_ep_" + str(epi) + "_SASR_step" + try: + data = np.load(filename) + except IOError: + import sys + sys.exit("Error: " + filename + " not found") + + s0 = int(data[0, 0]) + for step in range(np.size(data, 0)): + s = int(data[step, 0]) + a = int(data[step, 1]) + sp = int(data[step, 2]) + rew = data[step, 3] + ty_re = np.where(task.REWARDS == rew)[0][0] + freq_t[s, a, sp] += 1 + freq_r[s, a, sp, ty_re] += 1 + + # normalize + for s in range(task.n_states): + if s % 100.0 == 0: + print("state ", str(s), " of ", str(task.n_states)) + for a in range(task.n_actions): + partial_sum_t = np.sum(freq_t[s, a, :]) # np.sum(freq_t, 2) + for sp in range(task.n_states): + if partial_sum_t == 0: + t[s, a, sp] = 1.0 / task.n_states + else: + t[s, a, sp] = (freq_t[s, a, sp] / partial_sum_t) + + # Reward function: + partial_sum_r = np.sum(freq_r[s, a, sp, :]) # np.sum(Freq_R,3) + for ty_re in range(task.REWARDS.size): + if partial_sum_r == 0: + r[s, a, sp, ty_re] = 1.0 / task.REWARDS.size + else: + r[s, a, sp, ty_re] = (freq_r[s, a, sp, ty_re] / + partial_sum_r) + return + + +def get_sp(s, a): + """ return reached state from model """ + sp = -1 + # random.seed() + rd = random.random() + accum = 0 + for i in range(task.n_states): + accum = accum + t[s, a, i] + if rd < accum: + sp = i + break + if sp == -1: + print("\n Warning: Model lacks data for T in state: %d" % s + "\n") + return sp + + +def get_r(s, a, sp): + """ return obtained reward from model """ + reward = 0 + # random.seed() + rd = random.random() + accum = 0 + for i in range(task.REWARDS.size): + accum = accum + r[s, a, sp, i] + if rd < accum: + reward = task.REWARDS[i] + break + return reward + + +def load(filename, n_episodes_model=1): + """ Load model (T,R) from _model.npz. Update t, r, s0 + if no model is available, generate and save from SASR_step file """ + global t, r, s0 + file_model = filename + ".npz" + if os.path.isfile(file_model): + print("Model file found") + with np.load(file_model) as fm: + t = fm['T'] + r = fm['R'] + s0 = fm['s0'] + else: + print("Model file not found") + generate_t_and_r(filename, n_episodes_model) # create t, r, s0 + """ Save model (T,R) to _model.npz """ + np.savez_compressed(file_model, T=t, R=r, s0=s0) + return diff --git a/remoteApi.so b/remoteApi.so new file mode 100755 index 0000000..08e08d5 Binary files /dev/null and b/remoteApi.so differ diff --git a/rl_ros.py b/rl_ros.py new file mode 100755 index 0000000..b74802e --- /dev/null +++ b/rl_ros.py @@ -0,0 +1,261 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Link with ROS v 0.7 Giraff robot """ + +import copy +import sys +import time +from threading import Thread, RLock + +import numpy as np + +try: + import rospy + # from std_msgs.msg import String + # from my_python.srv import * + from nav_msgs.msg import Odometry + from sensor_msgs.msg import LaserScan + from geometry_msgs.msg import Twist + from std_srvs.srv import Empty +except ImportError: + rospy = None + Odometry = None + LaserScan = None + Twist = None + Empty = None + sys.exit("\n Please configure your ROS Environment " + "e.g: $ . ~/catkin_ws/devel/setup.bash") + +NODE_NAME = "rl_ros" + +HAS_KINECT = False +HAS_ARM = False + +N_DISTANCES = 8 # do not modify +N_LASERS = N_DISTANCES # rl_vrep compatible +# distances measured by lasers: +distance_obstacle = np.full(N_DISTANCES, -1, dtype=np.float64) +# base pose 2D: +mobilebase_pose2d = np.full(3, -1, dtype=np.float64) # x(m), y(m), theta(rad) + +twist = Twist() +twist.linear.x = 0 +twist.linear.y = 0 +twist.linear.z = 0 +twist.angular.x = 0 +twist.angular.y = 0 +twist.angular.z = 0 + +pub_cmd_vel = None +srv_reset_positions = None + +mutex_odom = RLock() +mutex_base_scan = RLock() +mutex_twist = RLock() + + +def setup(): + """ Setup ROS node. Create listener and talker threads """ + global pub_cmd_vel + global srv_reset_positions + + print("\n Initiating ROS Node") + + rospy.init_node(NODE_NAME, anonymous=True) + # ToDo: Change to 'odom' in ROS launch for Giraff: + rospy.Subscriber('odom_giraff', Odometry, callback_odom) + rospy.Subscriber('laser_scan', LaserScan, callback_base_scan) + pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=1, + tcp_nodelay=True) + + srv_reset_positions = rospy.ServiceProxy('reset_positions', Empty) + + thread_listener = Thread(target=listener, args=[]) + thread_talker = Thread(target=talker, args=[]) + thread_listener.start() + thread_talker.start() + + rospy.loginfo("Node Initiated: %s", NODE_NAME) + return + + +def callback_odom(data): + """ callback from odometry subscribed topic. Update mobilebase_pose2d """ + global mobilebase_pose2d + mutex_odom.acquire() + mobilebase_pose2d[0] = data.pose.pose.position.x + mobilebase_pose2d[1] = data.pose.pose.position.y + mobilebase_pose2d[2] = data.pose.pose.orientation.z + mutex_odom.release() + return + + +def callback_base_scan(data): + """ callback from odometry subscribed topic. Update distance_obstacle """ + global distance_obstacle + laser_data = data.ranges + n_data = len(laser_data) + mutex_base_scan.acquire() + # Get specific points from Hokuyo laser (240º) + distance_obstacle[0] = laser_data[int(n_data / 2)] + distance_obstacle[1] = laser_data[int(n_data / 2 + 45)] + distance_obstacle[2] = laser_data[int(n_data / 2 + 90)] + distance_obstacle[3] = laser_data[n_data - 1] + distance_obstacle[4] = 50 # no rear laser + distance_obstacle[5] = laser_data[0] + distance_obstacle[6] = laser_data[int(n_data / 2 - 90)] + distance_obstacle[7] = laser_data[int(n_data / 2 - 45)] + distance_obstacle[distance_obstacle > 50] = 50 + distance_obstacle[np.isnan(distance_obstacle)] = 50 + mutex_base_scan.release() + len(laser_data) + return + + +def listener(): + """ Call rospy spin for subscribed topics""" + rospy.spin() + return + + +def talker(): + """ Publish ROS data """ + rate = rospy.Rate(10) # 10hz + while not rospy.is_shutdown(): + mutex_twist.acquire() + pub_cmd_vel.publish(twist) + mutex_twist.release() + rate.sleep() + return + + +def finish(): + """ Close ROS node """ + print("\nFinishing ROS Node \n") + rospy.signal_shutdown("Finished from RL-ROBOT") + return + + +def move_wheels(left_speed, right_speed): + """ Convert wheel speeds into (v,w) and update published speed values """ + # rospy.init_node('listener', anonymous=True) + v_speed, w_speed = wheel_to_vw_speed(left_speed, right_speed) + + if any(distance_obstacle < 0.18): + if v_speed > 0: + v_speed = 0 + w_speed *= 5 # hand tuned for giraff robot + + mutex_twist.acquire() + global twist + twist.linear.x = v_speed + twist.linear.y = 0 + twist.linear.z = 0 + twist.angular.x = 0 + twist.angular.y = 0 + twist.angular.z = w_speed + mutex_twist.release() + return + + +def wheel_to_vw_speed(left_speed, right_speed): + """ Return v,w speeds of the robot from + left, right wheel speeds in rad/s """ + d = 0.15 # wheels radius of Giraff + s = 0.46 # separation between wheels of Giraff + v_left = left_speed * d / 2 + v_right = right_speed * d / 2 + v = (v_left + v_right) / 2 + w = (v_right - v_left) / s + # print("v:",v," w:",w") + return v, w + + +def get_mobilebase_pose2d(): + """ Returns the pose of the robot: [ x(m), y(m), Theta(rad) ] """ + mutex_odom.acquire() + pos = mobilebase_pose2d + mutex_odom.release() + copy_pos = copy.copy(pos) # fix shared memory issues + return copy_pos + + +def get_distance_obstacle(): + """ Returns an array of distances measured by lasers (m) """ + mutex_base_scan.acquire() + dist = distance_obstacle + mutex_base_scan.release() + copy_dist = copy.copy(dist) # fix shared memory issues + return copy_dist + + +def stop_motion(): + """ Stops robot motion """ + move_wheels(0, 0) + + +def setup_devices(): + """ just for V-Rep Compatibility""" + pass + + +def connect(): + """ just for V-Rep Compatibility""" + return + + +def disconnect(): + """ just for V-Rep Compatibility""" + return + + +def start(): + """ just for V-Rep Compatibility""" + reset_robot() + time.sleep(0.5) + return + + +def stop(): + """ just for V-Rep Compatibility""" + reset_robot() + time.sleep(0.5) + return + + +def show_msg(message): + """ Print a message (ROS log) """ + rl_str = "rl_mapir: " + message + print(rl_str) + rospy.loginfo(rl_str) + return + + +def reset_robot(): + """ Call to a Reset ROS service if available """ + rospy.wait_for_service('reset_positions') + return + +# ToDo: Implement the following functions to use an arm in ROS + +# def moveArm(v): +# return +# +# def moveBiceps(v): +# return +# +# def moveForearm(v): +# return +# +# def stopArmAll(v): +# return +# +# def getGripperPose3d(): +# return np.array(pos) +# +# def getGoalPose3d(): +# return np.array(pos) diff --git a/rl_vrep.py b/rl_vrep.py new file mode 100755 index 0000000..c169742 --- /dev/null +++ b/rl_vrep.py @@ -0,0 +1,259 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Link with V-REP simulator """ +import time + +import numpy as np + +import vrep # Vrep API library + +WAIT_RESPONSE = False # True: Synchronous response (too much delay) + +LASER_DISTRIBUTION = ('sensor_front', 'sensor_front_left', 'sensor_left', + 'sensor_rear_left', 'sensor_rear', 'sensor_rear_right', + 'sensor_right', 'sensor_front_right') +HAS_KINECT = False +HAS_ARM = True +HAS_GOAL_OBJECT = True + +# V-REP data transmission modes: +WAIT = vrep.simx_opmode_oneshot_wait +ONESHOT = vrep.simx_opmode_oneshot +STREAMING = vrep.simx_opmode_streaming +BUFFER = vrep.simx_opmode_buffer + +if WAIT_RESPONSE: + MODE_INI = WAIT + MODE = WAIT +else: + MODE_INI = STREAMING + MODE = BUFFER + +N_LASERS = 8 # 1 point laser each + +robotID = -1 +laserID = [-1] * N_LASERS +left_motorID = -1 +right_motorID = -1 +clientID = -1 + +armID = -1 +bicepsID = -1 +forearmID = -1 +gripperID = -1 + +ballID = -1 # Goal object to reach for 2d navigation and 3d arm motion tasks + +kinect_rgb_ID = -1 # not used so far +kinect_depth_ID = -1 # not used so far + +distance = np.full(N_LASERS, -1, dtype=np.float64) # distances from lasers (m) +pose = np.full(3, -1, dtype=np.float64) # Pose 2d base: x(m), y(m), theta(rad) + + +def show_msg(message): + """ send a message for printing in V-REP """ + vrep.simxAddStatusbarMessage(clientID, message, WAIT) + return + + +def connect(): + """ Connect to the simulator""" + vrep.simxFinish(-1) # just in case, close all opened connections + global clientID + clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 3000, 5) + # Connect to V-REP + assert (clientID != -1), ("\n\n V-REP remote API server connection failed. " + "Is V-REP running?") + print('Connected to Robot') + show_msg('Python: Hello') + time.sleep(0.5) + return + + +def disconnect(): + """ Disconnect from the simulator""" + # Make sure that the last command sent has arrived + vrep.simxGetPingTime(clientID) + show_msg('RL-ROBOT: Bye') + # Now close the connection to V-REP: + vrep.simxFinish(clientID) + time.sleep(0.5) + return + + +def start(): + """ Start the simulation (force stop and setup)""" + stop() + setup_devices() + vrep.simxStartSimulation(clientID, ONESHOT) + time.sleep(0.5) + # Solve a rare bug in the simulator by repeating: + setup_devices() + vrep.simxStartSimulation(clientID, ONESHOT) + time.sleep(0.5) + return + + +def stop(): + """ Stop the simulation """ + vrep.simxStopSimulation(clientID, ONESHOT) + time.sleep(0.5) + + +def setup_devices(): + """ Assign the devices from the simulator to specific IDs """ + global robotID, left_motorID, right_motorID, laserID + global armID, bicepsID, forearmID, gripperID + global kinect_rgb_ID, kinect_depth_ID + global ballID + # rc: return_code (not used) + # robot + rc, robotID = vrep.simxGetObjectHandle(clientID, 'robot', WAIT) + # motors + rc, left_motorID = vrep.simxGetObjectHandle(clientID, 'leftMotor', WAIT) + rc, right_motorID = vrep.simxGetObjectHandle(clientID, 'rightMotor', WAIT) + # lasers + for idx, item in enumerate(LASER_DISTRIBUTION): + ec, laserID[idx] = vrep.simxGetObjectHandle(clientID, item, WAIT) + # arm + if HAS_ARM: + rc, armID = vrep.simxGetObjectHandle( + clientID, 'arm_joint', WAIT) + rc, bicepsID = vrep.simxGetObjectHandle( + clientID, 'biceps_joint', WAIT) + rc, forearmID = vrep.simxGetObjectHandle( + clientID, 'forearm_joint', WAIT) + rc, gripperID = vrep.simxGetObjectHandle( + clientID, 'gripper_1_visual', WAIT) + # Kinect + if HAS_KINECT: + rc, kinect_rgb_ID = vrep.simxGetObjectHandle( + clientID, 'kinect_rgb', WAIT) + rc, kinect_depth_ID = vrep.simxGetObjectHandle( + clientID, 'kinect_depth', WAIT) + # ball + if HAS_GOAL_OBJECT: + rc, ballID = vrep.simxGetObjectHandle(clientID, 'Ball', WAIT) + + # start up devices + + # wheels + vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING) + vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING) + # pose + vrep.simxGetObjectPosition(clientID, robotID, -1, MODE_INI) + vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE_INI) + # distances from lasers + for i in laserID: + vrep.simxReadProximitySensor(clientID, i, MODE_INI) + + if HAS_ARM: + vrep.simxSetJointTargetVelocity(clientID, armID, 0, STREAMING) + vrep.simxSetJointTargetVelocity(clientID, bicepsID, 0, STREAMING) + vrep.simxSetJointTargetVelocity(clientID, forearmID, 0, STREAMING) + vrep.simxGetObjectPosition(clientID, gripperID, -1, MODE_INI) + + if HAS_GOAL_OBJECT: + vrep.simxGetObjectPosition(clientID, ballID, -1, MODE_INI) + + if HAS_KINECT: + rc, resolution, image = vrep.simxGetVisionSensorImage( + clientID, kinect_rgb_ID, 0, MODE_INI) + im = np.array(image, dtype=np.uint8) + im.resize([resolution[1], resolution[0], 3]) + # plt.imshow(im, origin='lower') + # return_code, resolution, depth = vrep.simxGetVisionSensorImage( + # clientID, kinect_depth_ID, 0, MODE_INI) + # de = np.array(depth) + time.sleep(0.5) + return + + +def get_image_rgb(): + """ Get RGB image from a Kinect """ + rc, resolution, image = vrep.simxGetVisionSensorImage( + clientID, kinect_rgb_ID, 0, MODE) + + im = np.array(image, dtype=np.uint8) + im.resize([resolution[1], resolution[0], 3]) + # im.shape + # plt.imshow(im,origin='lower') + return im + + +def get_image_depth(): + """ get image Depth from a Kinect """ + rc, resolution, depth = vrep.simxGetVisionSensorImage( + clientID, kinect_depth_ID, 0, MODE) + de = np.array(depth) + return de + + +def get_mobilebase_pose2d(): + """ return the pose of the robot: [ x(m), y(m), Theta(rad) ] """ + rc, pos = vrep.simxGetObjectPosition(clientID, robotID, -1, MODE) + rc, ori = vrep.simxGetObjectOrientation(clientID, robotID, -1, MODE) + pos = np.array([pos[0], pos[1], ori[2]]) + return pos + + +def get_distance_obstacle(): + """ return an array of distances measured by lasers (m) """ + for i in range(0, N_LASERS): + rc, ds, detected_point, doh, dsn = vrep.simxReadProximitySensor( + clientID, laserID[i], MODE) + distance[i] = detected_point[2] + return distance + + +def move_wheels(v_left, v_right): + """ move the wheels. Input: Angular velocities in rad/s """ + vrep.simxSetJointTargetVelocity(clientID, left_motorID, v_left, STREAMING) + vrep.simxSetJointTargetVelocity(clientID, right_motorID, v_right, STREAMING) + return + + +def stop_motion(): + """ stop the base wheels """ + vrep.simxSetJointTargetVelocity(clientID, left_motorID, 0, STREAMING) + vrep.simxSetJointTargetVelocity(clientID, right_motorID, 0, STREAMING) + return + + +def move_arm(w): + """ move arm joint. Angular velocities in rad/s (+anticlockwise) """ + vrep.simxSetJointTargetVelocity(clientID, armID, w, STREAMING) + + +def move_biceps(w): + """ move biceps joint. Angular velocities in rad/s(+anticlockwise) """ + vrep.simxSetJointTargetVelocity(clientID, bicepsID, w, STREAMING) + + +def move_forearm(w): + """ move forearm joint. Angular velocities in rad/s (+anticlockwise) """ + vrep.simxSetJointTargetVelocity(clientID, forearmID, w, STREAMING) + + +def stop_arm_all(): + """ stop arm joints """ + move_arm(0) + move_biceps(0) + move_forearm(0) + + +def get_gripper_pose3d(): + """ Returns the position of the gripper: [ x(m), y(m), z(m) ] """ + rc, pos = vrep.simxGetObjectPosition(clientID, gripperID, -1, MODE) + return np.array(pos) + + +def get_goal_pose_3d(): + """ Returns the position of the goal object: [ x(m), y(m), z(m) ] """ + rc, pos = vrep.simxGetObjectPosition(clientID, ballID, -1, MODE) + return np.array(pos) diff --git a/rlrobot.py b/rlrobot.py new file mode 100755 index 0000000..e42c4fd --- /dev/null +++ b/rlrobot.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +-----------------------------------+-----------------------------------+ +# | RL-ROBOT | +# | | +# | Copyright (c) 2016, Individual contributors, see AUTHORS file. | +# | Machine Perception and Intelligent Robotics (MAPIR), | +# | University of Malaga. | +# | | +# | This program is free software: you can redistribute it and/or modify | +# | it under the terms of the GNU General Public License as published by | +# | the Free Software Foundation, either version 3 of the License, or | +# | (at your option) any later version. | +# | | +# | This program is distributed in the hope that it will be useful, | +# | but WITHOUT ANY WARRANTY; without even the implied warranty of | +# | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | +# | GNU General Public License for more details. | +# | | +# | You should have received a copy of the GNU General Public License | +# | along with this program. If not, see . | +# +-----------------------------------------------------------------------+ +""" RL-ROBOT main script """ + +import signal +import sys +import time +from shutil import copyfile + +import numpy as np + +import exp + +tasks_path = "tasks/" +results_path = "results/" + + +def run(): + """ Perform experiments: setups, executions, save results """ + import sys + if sys.version_info[0] < 3: + sys.exit("Sorry, Python 3 required") + + exp.check() # Check experiment parameters + + # copy the selected taskfile to speed up the execution: + copyfile("tasks/" + exp.TASK_ID + ".py", "task.py") + import task + import robot + import lp + import show + import save + + task.setup() + + caption = (exp.TASK_ID + "_" + exp.ALGORITHM + "_" + exp.ACTION_STRATEGY) + if exp.SUFFIX: + caption += "_" + exp.SUFFIX + + save.new_dir(results_path, caption) # create result directory + + epi = 0 + # Average Reward per step (aveR): + ave_r = np.zeros((exp.N_REPETITIONS, exp.N_STEPS)) + # Mean(aveR) of all tests per step + mean_ave_r = np.zeros(exp.N_STEPS) + # AveR per episode + epi_ave_r = np.zeros([exp.N_REPETITIONS, exp.N_EPISODES]) + # actual step time per episode (for computational cost only) + actual_step_time = np.zeros(exp.N_REPETITIONS) + + if exp.LEARN_FROM_MODEL: + import model + file_model = tasks_path + exp.FILE_MODEL + "/" + exp.FILE_MODEL + model.load(file_model, exp.N_EPISODES_MODEL) + else: + robot.connect() # Connect to V-REP / ROS + + if exp.CONTINUE_PREVIOUS_EXP: + prev_exp = __import__(exp.PREVIOUS_EXP_FILE) + print("NOTE: Continue experiments from: " + exp.PREVIOUS_EXP_FILE) + time.sleep(3) + + # Experiment repetition loop ------------------------------------------ + for rep in range(exp.N_REPETITIONS): + if exp.CONTINUE_PREVIOUS_EXP: + last_q, last_v = prev_exp.q, prev_exp.v + last_policy, last_q_count = prev_exp.policy, prev_exp.q_count + else: + last_q = last_v = last_policy = last_q_count = None + + # Episode loop ------------------ + for epi in range(exp.N_EPISODES): + + if exp.LEARN_FROM_MODEL: + print("Learning from Model") + task.STEP_TIME = 0 + lp.step_time = 0 + else: + robot.start() + + show.process_count(caption, rep, epi, exp.EPISODIC) + + lp.setup() # Learning process setup + + if (exp.EPISODIC and epi > 0) or exp.CONTINUE_PREVIOUS_EXP: + lp.q, lp.v = last_q, last_v + lp.policy, lp.count = last_policy, last_q_count + + lp.run() # Execute the learning process + + if not exp.LEARN_FROM_MODEL: + robot.stop() + + ave_r[rep] = lp.ave_r_step + actual_step_time[rep] = lp.actual_step_time + + if exp.EPISODIC: + last_q, last_v = lp.q, lp.v + last_policy, last_q_count = lp.policy, lp.q_count + + epi_ave_r[rep, epi] = lp.ave_r_step[lp.step] + + if exp.EXPORT_SASR_step: + save.simple(lp.sasr_step, "SASR_step") + + # end of episode + + show.process_remaining(rep, epi) + + mean_ave_r = np.mean(ave_r, axis=0) + + # End of experiment repetition loop ---------------------------- + + # Mean of AveR per step (last episode) + + save.plot_mean(mean_ave_r, epi) + + save.simple(ave_r, "aveR") + # If EPISODIC: Save ave_r of last episode + + if exp.EPISODIC: + # Mean of AveR reached (last step) per episode + mean_epi_ave_r = np.mean(epi_ave_r, axis=0) + save.plot_mean(mean_epi_ave_r, "ALL") + save.simple(epi_ave_r, "EPI") + + final_r = mean_ave_r[lp.step] + final_actual_step_time = np.mean(actual_step_time) + + save.log(final_r, final_actual_step_time) + save.arrays() + print("Mean average Reward = %0.2f" % final_r, "\n") + print("Mean actual step time (s): %0.6f" % final_actual_step_time, "\n") + + if not exp.LEARN_FROM_MODEL: + robot.disconnect() + + +# ------------------------------------------------------------------------------ +def signal_handler(sig, _): + """ capture Ctrl-C event and exists""" + sys.exit('\n Process interrupted (signal ' + str(sig) + ')') + + +# capture SIGINT when ROS threads are present: +signal.signal(signal.SIGINT, signal_handler) + +if __name__ == '__main__': + if exp.ENVIRONMENT_TYPE == "ROS": + print(" \n linking ROS ... ") + import rl_ros + + rl_ros.setup() # create ROS talker and listener threads + time.sleep(0.1) + run() + rl_ros.finish() # finish ROS threads + else: + run() diff --git a/robot.py b/robot.py new file mode 100755 index 0000000..16b6d0b --- /dev/null +++ b/robot.py @@ -0,0 +1,258 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Generic Robot module """ + +import math +import sys + +import numpy as np + +import exp + +link_dict = {'ROS': 'rl_ros', 'VREP': 'rl_vrep', 'MODEL': 'rl_vrep'} +try: + link_module = link_dict[exp.ENVIRONMENT_TYPE] +except KeyError: + sys.exit("ENVIRONMENT_TYPE " + exp.ENVIRONMENT_TYPE + " undefined \n") +link = __import__(link_module) + +# 2D navigation + +mobilebase_pose2d = np.full(3, -1, dtype=np.float64) # x(m),y(m),theta(rad) +last_mobilebase_pose2d = np.full(3, -1, dtype=np.float64) +mobilebase_displacement2d = 0 # between two consecutive steps (m) + +dist_obstacle = np.full(link.N_LASERS, -1, dtype=np.float64) + +last_distance_mobilebase_goal = -1 +distance_mobilebase_goal = -1 +mobilebase_goal_displacement2d = -1 + +# 3D motion +gripper_pose3d = np.full(3, -1, dtype=np.float64) +last_gripper_pose3d = np.full(3, -1, dtype=np.float64) +gripper_displacement3d = -1 + +goal_pose3d = np.full(3, -1, dtype=np.float64) +last_goal_pose3d = np.full(3, -1, dtype=np.float64) +goal_displacement3d = -1 # Just In case we want to move an object + +last_distance_gripper_goal = -1 +distance_gripper_goal = -1 +gripper_goal_displacement3d = -1 + +AGENT_ELEMENTS = [] +ENV_ELEMENTS = [] + +initiated = False + +sensor = {} + + +def setup(agent_elem, environment_elem): + """ initialize robot """ + global AGENT_ELEMENTS, ENV_ELEMENTS + global mobilebase_pose2d, last_mobilebase_pose2d, mobilebase_displacement2d + global dist_obstacle + global gripper_pose3d, last_gripper_pose3d, gripper_displacement3d + global goal_pose3d, last_goal_pose3d, goal_displacement3d + global last_distance_mobilebase_goal, distance_mobilebase_goal + global mobilebase_goal_displacement2d + global last_distance_gripper_goal, distance_gripper_goal + global gripper_goal_displacement3d + global initiated + + global sensor + + AGENT_ELEMENTS = agent_elem + ENV_ELEMENTS = environment_elem + + mobilebase_pose2d = np.full(3, -1, dtype=np.float64) + last_mobilebase_pose2d = np.full(3, -1, dtype=np.float64) + + mobilebase_displacement2d = 0 + + dist_obstacle = np.full(link.N_LASERS, -1, dtype=np.float64) + + gripper_pose3d = np.full(3, -1, dtype=np.float64) + last_gripper_pose3d = np.full(3, -1, dtype=np.float64) + gripper_displacement3d = -1 + + goal_pose3d = np.full(3, -1, dtype=np.float64) + last_goal_pose3d = np.full(3, -1, dtype=np.float64) + goal_displacement3d = -1 + + last_distance_mobilebase_goal = -1 + distance_mobilebase_goal = -1 + mobilebase_goal_displacement2d = -1 + + last_distance_gripper_goal = -1 + distance_gripper_goal = -1 + gripper_goal_displacement3d = -1 + + update() + update() # To obtain the first relative displacement + + return + + +def update(): + """ update robot & environment state (sensors, locations...) """ + global mobilebase_pose2d, last_mobilebase_pose2d, mobilebase_displacement2d + global dist_obstacle + global gripper_pose3d, last_gripper_pose3d, gripper_displacement3d + global goal_pose3d, last_goal_pose3d, goal_displacement3d + global last_distance_mobilebase_goal, distance_mobilebase_goal + global mobilebase_goal_displacement2d + global last_distance_gripper_goal, distance_gripper_goal + global gripper_goal_displacement3d + + global sensor + + if "DISTANCE_SENSOR" in AGENT_ELEMENTS: + dist_obstacle = get_distance_obstacle() + + if "MOBILE_BASE" in AGENT_ELEMENTS: + last_mobilebase_pose2d = mobilebase_pose2d + mobilebase_pose2d = get_mobilebase_pose2d() + mobilebase_displacement2d = distance2d(mobilebase_pose2d, + last_mobilebase_pose2d) + + if "ARM" in AGENT_ELEMENTS: + last_gripper_pose3d = gripper_pose3d + gripper_pose3d = link.get_gripper_pose3d() + gripper_displacement3d = distance3d(gripper_pose3d, last_gripper_pose3d) + + if "GOAL_OBJECT" in ENV_ELEMENTS: + last_goal_pose3d = link.get_goal_pose_3d() + goal_pose3d = link.get_goal_pose_3d() + goal_displacement3d = distance3d(goal_pose3d, last_goal_pose3d) + + if "MOBILE_BASE" in AGENT_ELEMENTS: + last_distance_mobilebase_goal = distance_mobilebase_goal + distance_mobilebase_goal = distance2d(goal_pose3d, + mobilebase_pose2d) + mobilebase_goal_displacement2d = (distance_mobilebase_goal - + last_distance_mobilebase_goal) + # Negative: the mobilebase is getting closer + + if "ARM" in AGENT_ELEMENTS: + last_distance_gripper_goal = distance_gripper_goal + distance_gripper_goal = distance3d(goal_pose3d, gripper_pose3d) + gripper_goal_displacement3d = (distance_gripper_goal - + last_distance_gripper_goal) + # Negative: the arm is getting closer + + sensor["mobile_x"] = mobilebase_pose2d[0] + sensor["mobile_y"] = mobilebase_pose2d[1] + sensor["mobile_theta"] = mobilebase_pose2d[2] + + sensor["gripper_x"] = gripper_pose3d[0] + sensor["gripper_y"] = gripper_pose3d[1] + sensor["gripper_z"] = gripper_pose3d[2] + + sensor["goal_x"] = goal_pose3d[0] + sensor["goal_y"] = goal_pose3d[0] + + sensor["laser_front"] = dist_obstacle[0] + sensor["laser_front_left"] = dist_obstacle[1] + sensor["laser_left"] = dist_obstacle[2] + sensor["laser_rear_left"] = dist_obstacle[3] + sensor["laser_rear"] = dist_obstacle[4] + sensor["laser_rear_right"] = dist_obstacle[5] + sensor["laser_right"] = dist_obstacle[6] + sensor["laser_front_right"] = dist_obstacle[7] + + +def move_wheels(left_wheel, right_wheel): + """ move base wheels (inputs: rad/s) """ + link.move_wheels(left_wheel, right_wheel) + return + + +def stop_motion(): + """ stop the mobilebase """ + link.stop_motion() + return + + +def move_full_arm(arm, biceps, forearm): + """ move robotic arm """ + link.move_arm(arm) + link.move_biceps(biceps) + link.move_forearm(forearm) + return + + +def get_distance_obstacle(): + """ return distances to objects measured by laser """ + di = link.get_distance_obstacle() + return di + + +def get_mobilebase_pose2d(): + """ return 2d pose of the mobilebase (x,y,theta) """ + po = link.get_mobilebase_pose2d() + return po + + +def get_gripper_pose3d(): + """ return the position of the gripper: [ x(m), y(m), z(m) ] """ + po = link.get_gripper_pose3d() + return po + + +def get_goal_pose3d(): + """ return the position of the goal object: [ x(m), y(m), z(m) ] """ + po = link.get_goal_pose_3d() + return po + + +def distance2d(pose_a, pose_b): + """ get distance 2d (x,y axis) between 2 poses """ + delta_pose2d = abs(pose_a - pose_b) + displacement2d = math.sqrt(delta_pose2d[0] ** 2 + delta_pose2d[1] ** 2) + return displacement2d + + +def distance3d(pose_a, pose_b): + """ get distance 3d between 2 poses """ + + delta_pose3d = abs(pose_a - pose_b) + displacement3d = math.sqrt(delta_pose3d[0] ** 2 + delta_pose3d[1] ** 2 + + delta_pose3d[2] ** 2) + return displacement3d + + +def start(): + """ Start robot """ + link.start() + return + + +def stop(): + """ stop robot """ + link.stop() + return + + +def setup_devices(): + """ setup robot's devices """ + link.setup_devices() + return + + +def connect(): + """ connect to the robot """ + link.connect() + return + + +def disconnect(): + """ disconnect from the robot """ + link.disconnect() + return diff --git a/save.py b/save.py new file mode 100755 index 0000000..08545c8 --- /dev/null +++ b/save.py @@ -0,0 +1,163 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Save resulting log, arrays and charts """ + +import os +import time + +import matplotlib.pyplot as plt +import numpy as np + +import exp +import lp +import task + +N_BINS = 6 + +path = None # Save path (including prefix of filename) + + +def new_dir(results_path, caption): + """ Create directory in which results will be saved """ + global path + + if not os.path.exists(results_path): + os.makedirs(results_path) + string_date = time.strftime("%Y_%m_%d_%H_%M", time.gmtime()) + folder = results_path + "/" + string_date + "_" + caption + if not os.path.exists(folder): + os.makedirs(folder) + path = folder + "/" + caption + + +def simple(data, suffix): + """ Save 'data' in file 'filename'. If the data is a numpy array, it is + saved as binary (.npy) """ + filename = path + "_" + suffix if suffix else path + if suffix: + filename += "_" + suffix + if type(data) is np.ndarray: + np.save(filename, data) + else: + filename += ".py" + with open(filename, 'w') as f: + f.write(data) + + +def log(mean_ave_r, mean_actual_step_time): + """ Save logfile (textfile)""" + + txt = '""" RL-ROBOT log. ' + txt += time.strftime("%d %b %Y %H:%M", time.gmtime()) + '"""\n\n' + # Save Parameters + txt += "# EXPERIMENT PARAMETERS\n" + txt += "TASK_ID = '" + exp.TASK_ID + "'\n" + txt += "ENVIRONMENT_TYPE = '" + exp.ENVIRONMENT_TYPE + "'\n" + txt += "SPEED_RATE = " + str(exp.SPEED_RATE) + "\n" + txt += "N_REPETITIONS = " + str(exp.N_REPETITIONS) + "\n" + txt += "N_EPISODES = " + str(exp.N_EPISODES) + "\n" + txt += "N_STEPS = " + str(exp.N_STEPS) + "\n\n" + + txt += "CONTINUE_PREVIOUS_EXP = " + str(exp.CONTINUE_PREVIOUS_EXP) + "\n" + txt += "PREVIOUS_EXP_FILE = '" + exp.PREVIOUS_EXP_FILE + "'\n" + txt += "FILE_MODEL = '" + exp.FILE_MODEL + "'\n" + txt += "N_EPISODES_MODEL = " + str(exp.N_EPISODES_MODEL) + "\n\n" + + txt += "ALGORITHM = '" + exp.ALGORITHM + "'\n" + txt += "ACTION_STRATEGY = '" + exp.ACTION_STRATEGY + "'\n" + txt += "GAMMA = " + str(exp.GAMMA) + "\n" + txt += "ALPHA = " + str(exp.ALPHA) + "\n" + txt += "LAMBDA = " + str(exp.LAMBDA) + "\n" + txt += "TEMPERATURE = " + str(exp.TEMPERATURE) + "\n\n" + + txt += "# TASK PARAMETERS \n" + txt += "TASK_NAME = '" + task.NAME + "'\n" + txt += "TASK_DESCRIPTION = '" + task.DESCRIPTION + "'\n" + txt += "TASK_ROBOT = '" + task.ROBOT + "'\n" + txt += "TASK_ENV = '" + task.ENVIRONMENT + "'\n" + txt += "TASK_ENV_DETAIL = '" + task.ENVIRONMENT_DETAIL + "'\n" + txt += "TASK_AGENT_ELEMENTS = " + str(task.AGENT_ELEMENTS) + "\n" + txt += ("TASK_ENV_ELEMENTS = " + str(task.ENV_ELEMENTS) + "\n\n") + + txt += "STEP_TIME = " + str(task.STEP_TIME) + "\n" + txt += "MOTOR_SPEED = " + str(task.MOTOR_SPEED) + "\n" + txt += "RANGE_OBSTACLES = " + str(task.RANGE_OBSTACLES) + "\n" + txt += "RANGE_DISPLACEMENT = " + str(task.RANGE_DISPLACEMENT) + "\n" + txt += "RANGE_DAMAGE = " + str(task.RANGE_DAMAGE) + "\n\n" + + txt += "N_STATES = " + str(task.n_states) + "\n" + txt += "N_INPUTS = " + str(task.n_inputs) + "\n" + txt += "INPUT_NAMES = " + str(task.in_names) + "\n\n" + + txt += "N_ACTIONS = " + str(task.n_actions) + "\n" + txt += "N_OUTPUTS = " + str(task.n_outputs) + "\n" + txt += "OUTPUT_NAMES = " + str(task.out_names) + "\n\n" + + txt += "TASK_REWARDS = " + str(list(task.REWARDS)) + "\n" + txt += "INITIAL_STATE = " + str(lp.s0) + "\n\n" + + # Save results + txt += "# RESULTS\n" + txt += "Elapsed_time = %0.2f" % lp.elapsed_time + "\n" + txt += "Mean_actual_step_time = %0.6f" % mean_actual_step_time + "\n\n" + txt += "Mean_Average_reward = %0.2f" % mean_ave_r + "\n" + + filename = path + "_log.py" + with open(filename, 'w') as f: + f.write(txt) + + +def arrays(suffix=""): + """ Save resulting arrays: Policy, V, Q, Q_count (textfile)""" + filename = path + "_" + suffix if suffix else path + np.savez_compressed(filename, Policy=lp.policy, V=lp.v, Q=lp.q, + Q_count=lp.q_count) + + +def plot_simple(data, suffix="", tittle=""): + """ Plot simple average Reward per step """ + filename = path + "_" + suffix if suffix else path + plt.figure() + plt.plot(data) + plt.axis([0, data.size, -5, 10]) + plt.xlabel('STEP') + plt.ylabel('AVERAGE REWARD') + plt.title(tittle) + plt.savefig(filename) + return + + +def plot_simple_epi(data, suffix="", tittle=""): + """ Plot simple average Reward per episode) """ + filename = path + "_" + suffix if suffix else path + plt.figure() + plt.plot(data) + plt.axis([0, data.size - 1, -5, 10]) + plt.xlabel('EPISODE') + plt.ylabel('AVERAGE REWARD') + plt.title(tittle) + plt.savefig(filename) + return + + +def plot_mean(data, epi): + """ Plot mean of average Reward per step """ + filename = path + "_ep" + str(epi) if exp.EPISODIC else path + tittle = exp.TASK_ID + labl = exp.ALGORITHM + " " + exp.ACTION_STRATEGY + " " + exp.SUFFIX + plt.figure() + plt.plot(data, label=labl) + plt.axis([0, int(data.size) - 1, -5, 10]) + if epi == "ALL": + plt.xlabel('EPISODE') + else: + plt.xlabel('STEP') + plt.ylabel('MEAN of average Reward') + plt.title(tittle + " (Mean x" + str(exp.N_REPETITIONS) + ")") + plt.legend(loc='lower right', numpoints=1) + plt.savefig(filename) + return diff --git a/show.py b/show.py new file mode 100755 index 0000000..6f9b8e8 --- /dev/null +++ b/show.py @@ -0,0 +1,43 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Print custom messages """ + +import exp +import lp + + +def process_count(caption, rep, epi, episodic): + """ print current repetition and episode """ + print("-" * 50) + print(caption) + if exp.N_REPETITIONS > 1: + print('repetition \t{0} of {1}'.format(rep + 1, exp.N_REPETITIONS)) + if episodic: + print('episode \t{0} of {1}'.format(epi + 1, exp.N_EPISODES)) + print("-" * 50) + + +def process_remaining(rep, epi): + """ print remaining processes """ + if rep < exp.N_REPETITIONS - 1: + remaining_processes = (exp.N_REPETITIONS * exp.N_EPISODES - + rep * exp.N_EPISODES + epi + 1) + remaining_time = remaining_processes * lp.elapsed_time + print("Remaining time: \t {0:.2f}m ({1:.2f}h)".format( + remaining_time / 60.0, remaining_time / 3600.0), "\n") + + +def process_summary(): + """ print process summary """ + print("-" * 22, "END", "-" * 23) + print("Number of steps: \t", str(lp.step + 1)) + print("Actual Step Time: \t %.6f" % lp.actual_step_time + "s") + print("Elapsed Time:\t\t %.2f" % lp.elapsed_time, + "s ( %.2f" % (lp.elapsed_time / 60), + "m %.2f" % (lp.elapsed_time / 60 / 60), "h )") + print("Average Reward: \t %.2f" % lp.final_average_reward) + print("-" * 50, "\n") diff --git a/task.py b/task.py new file mode 100644 index 0000000..97ac9e9 --- /dev/null +++ b/task.py @@ -0,0 +1,120 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "Wander_1024s_9a" +DESCRIPTION = "Wander avoiding obstacles. 5 lasers. 4 ranges each. 9 actions" +ROBOT = "Pioneer 3dx with 8-point laser" +ENVIRONMENT = "VREP_SIM: square 6x6m with obstacles" +ENVIRONMENT_DETAIL = "SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming" +AGENT_ELEMENTS = ["MOBILE_BASE", "DISTANCE_SENSOR"] +ENV_ELEMENTS = [] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR","GOAL","ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: +STEP_TIME = 1 # s +MOTOR_SPEED = 1 # rad/s (pioneer 3dx: 1 rad/s: ~ 0.1m/s) +RANGE_OBSTACLES = 0.80 # m +RANGE_DISPLACEMENT = 0.07 # m +RANGE_DAMAGE = 0.05 # m + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "laser_front": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_right": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_right": np.linspace(0, RANGE_OBSTACLES, 4) +} +OUTPUT_VARIABLES = { + "left_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3), + "right_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + v_left, v_right = actuator[0], actuator[1] + # backward action is replaced by double speed + # 2 actions with only one wheel backwards changed to no motion (duplicated) + if v_left < 0 and v_right < 0: + v_left = v_right = MOTOR_SPEED * 2 + elif v_left * v_right < 0: + v_left = v_right = 0 + robot.move_wheels(v_left, v_right) # left wheel, right_wheel speeds (rad/s) + + +# TASK DEFINITION: REWARDS ---------------------------------------------------- +REWARDS = np.array([-10.0, -2.0, -0.02, 10.0]) +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + distance_fl = robot.sensor["laser_front_left"] + distance_fr = robot.sensor["laser_front_right"] + distance_f = robot.sensor["laser_front"] + displacement = robot.mobilebase_displacement2d + + n_collisions = (int(distance_fl < RANGE_DAMAGE) + + int(distance_fr < RANGE_DAMAGE) + + int(distance_f < RANGE_DAMAGE)) + r = REWARDS[2] + if n_collisions > 1: # big penalty + r = min(REWARDS) + elif n_collisions == 1: # small penalty + r = REWARDS[1] + elif displacement > RANGE_DISPLACEMENT: + r = max(REWARDS) + + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/arm_1k.py b/tasks/arm_1k.py new file mode 100644 index 0000000..9805ee4 --- /dev/null +++ b/tasks/arm_1k.py @@ -0,0 +1,116 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "Arm_cube_table_1024s9a" +DESCRIPTION = "Arm reaching a Cube on table (x,y) with 2 joints" +ROBOT = "Widowx arm" +ENVIRONMENT = "VREP_SIM: table with goal object" +ENVIRONMENT_DETAIL = "SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming" +AGENT_ELEMENTS = ["ARM"] +ENV_ELEMENTS = ["GOAL_OBJECT"] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR", "ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: (tuned for Widowx arm at V-REP scenario) +STEP_TIME = 1 # s +MOTOR_SPEED = 1 +RANGE_OBSTACLES = -1 # m (not used) +RANGE_DISPLACEMENT = 0.02 # m +RANGE_DAMAGE = -1 # m (not used) + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "gripper_x": np.linspace(5.2, 5.5, 4), # data from V-REP + "gripper_y": np.linspace(4.8, 5.2, 8), # (smaller partition) + "goal_x": np.linspace(5.2, 5.5, 4), + "goal_y": np.linspace(4.8, 5.2, 8), +} +OUTPUT_VARIABLES = { + "arm": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3), + "forearm": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + v_arm, v_forearm = actuator + v_biceps = 0 # Not used here + robot.move_full_arm(v_arm, v_biceps, v_forearm) + + +# TASK DEFINITION: REWARDS ---------------------------------------------------- +REWARDS = np.array([-10.0, -2.0, -1.0, -0.02, 1.0, 2.0, 3.0, 4.0, 5.0, 10.0]) +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + distance_gripper_goal = robot.distance_gripper_goal + displacement_gripper_goal = robot.gripper_goal_displacement3d + # if negative: Gripper is getting closer to the goal + + r = REWARDS[3] # tiny step penalty + + if distance_gripper_goal < 0.05: # robot reaches the goal + r = max(REWARDS) + # LE.step_time = 0 # here, if the robot reaches the static goal + # we can speed up the rest of the experiment + + elif displacement_gripper_goal > RANGE_DISPLACEMENT: # moving away + r = REWARDS[2] + + elif displacement_gripper_goal < -RANGE_DISPLACEMENT: # approaching + r = round(REWARDS[4] + 4 / distance_gripper_goal) + r = r if r < 5 else 5 + + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/arm_1k_model/arm_1k_model.npz b/tasks/arm_1k_model/arm_1k_model.npz new file mode 100644 index 0000000..7e5e850 Binary files /dev/null and b/tasks/arm_1k_model/arm_1k_model.npz differ diff --git a/tasks/arm_1k_model/arm_1k_model_SASR_step.zip b/tasks/arm_1k_model/arm_1k_model_SASR_step.zip new file mode 100644 index 0000000..e03607a Binary files /dev/null and b/tasks/arm_1k_model/arm_1k_model_SASR_step.zip differ diff --git a/tasks/arm_1k_model/arm_1k_model_log.py b/tasks/arm_1k_model/arm_1k_model_log.py new file mode 100644 index 0000000..4e5cb97 --- /dev/null +++ b/tasks/arm_1k_model/arm_1k_model_log.py @@ -0,0 +1,40 @@ +""" EXPERIMENT LOG """ + +# PARAMETERS +NAME = 'Arm_cube_table_1024s9a' +DESCRIPTION = 'Arm reaching a Cube on table grid 4x8 ' +ALGORITHM_NAME = 'LE_TOSL' +EXPLORATION_STRATEGY = 'SOFT' +ROBOT = 'Widowx Arm' +LEARN_FROM_MODEL = 'False' +ENVIRONMENT = 'VREP_SIM: Table-Cube' +ENVIRONMENT_DETAIL = 'SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming' +AGENT_ELEMENTS = ['ARM'] +ENVIRONMENT_ELEMENTS = ['GOAL_OBJECT'] +N_STATES = 1024 +N_ACTIONS = 9 +GAMMA = 0.9 +INITIAL_ALPHA = 0.1 +STEP_TIME = 0.333333333333 +INITIAL_POLICY = 0 + +steps = 120 +N_REPETITIONS = 1 +N_EPISODES = 50 + +FILE_MODEL = 'Arm_cube_table_1024s9a__LE_TOSL__SOFT__T3' + +s0 = 190 + +# REWARDS = [-10. -2. -1. -0.02 1. 2. 3. 4. 5. 10. ] + +MOTOR_POWER = 1 + +# RESULTS + +Mean_Average_reward = 9.87 + +Median_Average_reward = 9.87 + +Elapsed_time = 40.14 +actual_step_time = 0.33 diff --git a/tasks/arm_1k_model/arm_1k_model_results.zip b/tasks/arm_1k_model/arm_1k_model_results.zip new file mode 100644 index 0000000..24d7a0b Binary files /dev/null and b/tasks/arm_1k_model/arm_1k_model_results.zip differ diff --git a/tasks/arm_4k.py b/tasks/arm_4k.py new file mode 100644 index 0000000..ccd8a90 --- /dev/null +++ b/tasks/arm_4k.py @@ -0,0 +1,118 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "Arm_cube_table_4096s27a" +DESCRIPTION = "Arm reaching a Cube on table (x,y,z) with 3 joints" +ROBOT = "Widowx arm" +ENVIRONMENT = "VREP_SIM: table with goal object" +ENVIRONMENT_DETAIL = "SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming" +AGENT_ELEMENTS = ["ARM"] +ENV_ELEMENTS = ["GOAL_OBJECT"] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR", "ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: (tuned for Widowx arm at V-REP scenario) +STEP_TIME = 1 # s +MOTOR_SPEED = 1 +RANGE_OBSTACLES = -1 # m (not used) +RANGE_DISPLACEMENT = 0.02 # m +RANGE_DAMAGE = -1 # m (not used) + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "gripper_x": np.linspace(5.2, 5.5, 4), # Data from V-REP scenario + "gripper_y": np.linspace(4.8, 5.2, 8), # (smaller partition) + "gripper_z": np.linspace(0.2, 0.4, 4), + "goal_x": np.linspace(5.2, 5.5, 4), + "goal_y": np.linspace(4.8, 5.2, 8), +} +OUTPUT_VARIABLES = { + "arm": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3), + "biceps": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3), + "forearm": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + v_arm, v_biceps, v_forearm = actuator + # (modify joints if necessary) + robot.move_full_arm(v_arm, v_biceps, v_forearm) + + +# TASK DEFINITION: REWARDS ---------------------------------------------------- +REWARDS = np.array([-1.0, -0.02, 1.0, 2.0, 3.0, 4.0, 5.0, 10.0]) +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + distance_gripper_goal = robot.distance_gripper_goal + displacement_gripper_goal = robot.gripper_goal_displacement3d + # if negative: Gripper is getting closer to the goal + + r = REWARDS[1] # tiny step penalty + + if distance_gripper_goal < 0.05: # robot reaches the goal + r = max(REWARDS) + # LE.step_time = 0 # here, if the robot reaches the static goal + # we can speed up the rest of the experiment + + elif displacement_gripper_goal > RANGE_DISPLACEMENT: # moving away + r = min(REWARDS) + + elif displacement_gripper_goal < -RANGE_DISPLACEMENT: # approaching + r = round(REWARDS[2] + 4 / distance_gripper_goal) + r = r if r < 5 else 5 + + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/nav_1k.py b/tasks/nav_1k.py new file mode 100644 index 0000000..b5db67f --- /dev/null +++ b/tasks/nav_1k.py @@ -0,0 +1,152 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import math + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "navigation_2D_1024s_9a" +DESCRIPTION = "Navigation to a Goal. 16 pos / 8 orient / 8 lasers states" +ROBOT = "Pioneer 3dx with 8-point laser" +ENVIRONMENT = "VREP_SIM: square 4x4m with obstacles and a goal" +ENVIRONMENT_DETAIL = "SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming" +AGENT_ELEMENTS = ["MOBILE_BASE", "DISTANCE_SENSOR"] +ENV_ELEMENTS = ["GOAL_OBJECT"] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR", "ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: +STEP_TIME = 1 # s +MOTOR_SPEED = 1 # rad/s (pioneer 3dx: 1 rad/s: ~ 0.1m/s) +RANGE_OBSTACLES = 0.20 # m +RANGE_DISPLACEMENT = 0.07 # m +RANGE_DAMAGE = 0.08 # m + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "mobile_x": np.linspace(3, 7, 4), # V-REP scenario: (3 to 7)m in X + "mobile_y": np.linspace(3, 7, 4), # V-REP scenario: (3 to 7)m in Y + "mobile_theta": np.linspace(-math.pi, math.pi, 8), + "laser_front": np.linspace(0, RANGE_OBSTACLES, 2), + "laser_front_left": np.linspace(0, RANGE_OBSTACLES, 2), + "laser_front_right": np.linspace(0, RANGE_OBSTACLES, 2) +} +OUTPUT_VARIABLES = { + "left_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3), + "right_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + v_left, v_right = actuator[0], actuator[1] + # backward action is replaced by double speed + # 2 actions with only one wheel backwards changed to no motion (duplicated) + if v_left < 0 and v_right < 0: + v_left = v_right = MOTOR_SPEED * 2 + elif v_left * v_right < 0: + v_left = v_right = 0 + robot.move_wheels(v_left, v_right) # left wheel, right_wheel speeds (rad/s) + + +# TASK DEFINITION: REWARDS ---------------------------------------------------- +REWARDS = np.array([-10.0, -2.0, -1.0, -0.1, 1.0, 2.0, 3.0, 4.0, 5.0, 10.0]) +THRES_STEPS_AT_GOAL = 5 # auxiliary: if the robot remains at the goal more than +# this value, the step time will be reduced to accelerate the experiments +steps_at_goal = 0 +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + global goal_reached + global steps_at_goal + + distance_fl = robot.sensor["laser_front_left"] + distance_fr = robot.sensor["laser_front_right"] + distance_f = robot.sensor["laser_front"] + distance_robot_goal = robot.distance_mobilebase_goal + displacement_robot_goal = robot.mobilebase_goal_displacement2d + # if negative: MobileBase is getting closer + + n_collisions = (int(distance_fl < RANGE_DAMAGE) + + int(distance_fr < RANGE_DAMAGE) + + int(distance_f < RANGE_DAMAGE)) + + r = REWARDS[3] + + if distance_robot_goal < 0.5: # robot reaches the goal + r = max(REWARDS) + + elif n_collisions > 1: + r = min(REWARDS) # big penalty + + elif n_collisions == 1: # small penalty + r = REWARDS[1] + + elif displacement_robot_goal > RANGE_DISPLACEMENT: # moving away + r = REWARDS[2] + + elif displacement_robot_goal < -RANGE_DISPLACEMENT: # approaching + r = round(REWARDS[4] + 4 / distance_robot_goal) + r = r if r < 5 else 5 + + goal_reached = False + if r >= max(REWARDS): + steps_at_goal += 1 + if steps_at_goal > THRES_STEPS_AT_GOAL: + # LE.step_time = LE.INITIAL_STEP_TIME/8.0 + goal_reached = True + robot.stop_motion() + else: + steps_at_goal = 0 + + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/nav_1k_model/nav_1k_model.npz b/tasks/nav_1k_model/nav_1k_model.npz new file mode 100644 index 0000000..f133099 Binary files /dev/null and b/tasks/nav_1k_model/nav_1k_model.npz differ diff --git a/tasks/nav_1k_model/nav_1k_model_log.py b/tasks/nav_1k_model/nav_1k_model_log.py new file mode 100644 index 0000000..acf17be --- /dev/null +++ b/tasks/nav_1k_model/nav_1k_model_log.py @@ -0,0 +1,38 @@ +""" EXPERIMENT LOG """ + +# PARAMETERS +NAME = 'Nav_Maze4x4_obstacles_1024s9a' +DESCRIPTION = 'Navigation to a Goal. 16 pos / 8 orient / 8 lasers states' +ALGORITHM_NAME = 'LE_TOSL' +EXPLORATION_STRATEGY = 'SOFT' +ROBOT = 'Pioneer 3dx with 8-point laser' +LEARN_FROM_MODEL = 'False' +ENVIRONMENT = 'VREP_SIM: maze4X4_goal_with_obstacles' +ENVIRONMENT_DETAIL = 'SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming' +AGENT_ELEMENTS = ['MOBILE_BASE', 'DISTANCE_SENSOR'] +ENVIRONMENT_ELEMENTS = ['GOAL_OBJECT'] +N_STATES = 1024 +N_ACTIONS = 9 +GAMMA = 0.9 +INITIAL_ALPHA = 0.1 +STEP_TIME = 0.333333333333 +INITIAL_POLICY = 0 + +steps = 3600 +N_REPETITIONS = 1 +N_EPISODES = 50 + +FILE_MODEL = 'Nav_Maze4x4_obstacles_1024s9a__LE_TOSL__SOFT__MODEL_T2' + +s0 = 944 + +# REWARDS = [-10. -2. -1. -0.1 1. 2. 3. 4. 5. 10. ] + +MOTOR_POWER = 1 + +# RESULTS + +Average_reward = 9.48 + +Elapsed_time = 69.95 +actual_step_time = 0.02 diff --git a/tasks/nav_1k_model/nav_1k_model_result.zip b/tasks/nav_1k_model/nav_1k_model_result.zip new file mode 100644 index 0000000..79634b0 Binary files /dev/null and b/tasks/nav_1k_model/nav_1k_model_result.zip differ diff --git a/tasks/wander_1k.py b/tasks/wander_1k.py new file mode 100644 index 0000000..97ac9e9 --- /dev/null +++ b/tasks/wander_1k.py @@ -0,0 +1,120 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "Wander_1024s_9a" +DESCRIPTION = "Wander avoiding obstacles. 5 lasers. 4 ranges each. 9 actions" +ROBOT = "Pioneer 3dx with 8-point laser" +ENVIRONMENT = "VREP_SIM: square 6x6m with obstacles" +ENVIRONMENT_DETAIL = "SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming" +AGENT_ELEMENTS = ["MOBILE_BASE", "DISTANCE_SENSOR"] +ENV_ELEMENTS = [] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR","GOAL","ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: +STEP_TIME = 1 # s +MOTOR_SPEED = 1 # rad/s (pioneer 3dx: 1 rad/s: ~ 0.1m/s) +RANGE_OBSTACLES = 0.80 # m +RANGE_DISPLACEMENT = 0.07 # m +RANGE_DAMAGE = 0.05 # m + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "laser_front": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_right": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_right": np.linspace(0, RANGE_OBSTACLES, 4) +} +OUTPUT_VARIABLES = { + "left_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3), + "right_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + v_left, v_right = actuator[0], actuator[1] + # backward action is replaced by double speed + # 2 actions with only one wheel backwards changed to no motion (duplicated) + if v_left < 0 and v_right < 0: + v_left = v_right = MOTOR_SPEED * 2 + elif v_left * v_right < 0: + v_left = v_right = 0 + robot.move_wheels(v_left, v_right) # left wheel, right_wheel speeds (rad/s) + + +# TASK DEFINITION: REWARDS ---------------------------------------------------- +REWARDS = np.array([-10.0, -2.0, -0.02, 10.0]) +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + distance_fl = robot.sensor["laser_front_left"] + distance_fr = robot.sensor["laser_front_right"] + distance_f = robot.sensor["laser_front"] + displacement = robot.mobilebase_displacement2d + + n_collisions = (int(distance_fl < RANGE_DAMAGE) + + int(distance_fr < RANGE_DAMAGE) + + int(distance_f < RANGE_DAMAGE)) + r = REWARDS[2] + if n_collisions > 1: # big penalty + r = min(REWARDS) + elif n_collisions == 1: # small penalty + r = REWARDS[1] + elif displacement > RANGE_DISPLACEMENT: + r = max(REWARDS) + + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/wander_1k_GIRAFF.py b/tasks/wander_1k_GIRAFF.py new file mode 100644 index 0000000..2111685 --- /dev/null +++ b/tasks/wander_1k_GIRAFF.py @@ -0,0 +1,120 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "Wander_1024s_9a" +DESCRIPTION = "Wander avoiding obstacles. 5 lasers. 4 ranges each " +ROBOT = "Giraff" +ENVIRONMENT = "ROS" +ENVIRONMENT_DETAIL = "Trapezoid 1.5x1.0m" +AGENT_ELEMENTS = ["MOBILE_BASE", "DISTANCE_SENSOR"] +ENV_ELEMENTS = [] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR","GOAL","ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: (tuned for Giraff robot) +STEP_TIME = 1 # s +MOTOR_SPEED = 0.5 # rad/s (pioneer 3dx: 1 rad/s: ~ 0.1m/s) +RANGE_OBSTACLES = 0.25 # m +RANGE_DISPLACEMENT = 0.03 # m +RANGE_DAMAGE = 0.15 # m + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "laser_front": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_right": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_right": np.linspace(0, RANGE_OBSTACLES, 4) +} +OUTPUT_VARIABLES = { + "left_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3), + "right_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 3) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + v_left, v_right = actuator[0], actuator[1] + # backward action is replaced by double speed + # 2 actions with one wheel backwards changed to no motion (duplicated) + if v_left < 0 and v_right < 0: + v_left = v_right = MOTOR_SPEED * 2 + elif v_left * v_right < 0: + v_left = v_right = 0 + robot.move_wheels(v_left, v_right) # left wheel, right_wheel speeds (rad/s) + + +# TASK DEFINITION: REWARDS ---------------------------------------------------- +REWARDS = np.array([-10.0, -2.0, -0.02, 10.0]) +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + distance_fl = robot.sensor["laser_front_left"] + distance_fr = robot.sensor["laser_front_right"] + distance_f = robot.sensor["laser_front"] + displacement = robot.mobilebase_displacement2d + + n_collisions = (int(distance_fl < RANGE_DAMAGE) + + int(distance_fr < RANGE_DAMAGE) + + int(distance_f < RANGE_DAMAGE)) + + r = REWARDS[2] + if n_collisions > 1: # big penalty + r = min(REWARDS) + elif n_collisions == 1: # small penalty + r = REWARDS[1] + elif displacement > RANGE_DISPLACEMENT: + r = max(REWARDS) + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/wander_1k_model/wander_1k_model.npz b/tasks/wander_1k_model/wander_1k_model.npz new file mode 100644 index 0000000..9ed20cc Binary files /dev/null and b/tasks/wander_1k_model/wander_1k_model.npz differ diff --git a/tasks/wander_1k_model/wander_1k_model_SASR_step b/tasks/wander_1k_model/wander_1k_model_SASR_step new file mode 100644 index 0000000..4969dfd Binary files /dev/null and b/tasks/wander_1k_model/wander_1k_model_SASR_step differ diff --git a/tasks/wander_1k_model/wander_1k_model_log.py b/tasks/wander_1k_model/wander_1k_model_log.py new file mode 100644 index 0000000..e598c1e --- /dev/null +++ b/tasks/wander_1k_model/wander_1k_model_log.py @@ -0,0 +1,38 @@ +""" EXPERIMENT LOG """ + +# PARAMETERS +NAME = 'Wander_Maze6x6_1024s9a' +DESCRIPTION = 'Wander avoiding obstacles. 4 Distance Ranges with 5 Lasers' +ALGORITHM_NAME = 'LE_TOSL' +EXPLORATION_STRATEGY = 'SOFT' +ROBOT = 'Pioneer 3dx with 8-point laser' +ENVIRONMENT = 'VREP_SIM: maze 6x6' +ENVIRONMENT_DETAIL = 'SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming' +AGENT_ELEMENTS = ['MOBILE_BASE', 'DISTANCE_SENSOR'] +ENV_ELEMENTS = [] +N_STATES = 1024 +N_ACTIONS = 9 +GAMMA = 0.9 +INITIAL_ALPHA = 0.1 +STEP_TIME = 0.333333333333 +INITIAL_POLICY = 0 + +steps = 18000 +N_REPETITIONS = 1 +N_EPISODES = 1 + +LEARN_FROM_MODEL = False +FILE_MODEL = '' + +s0 = 1023 + +# REWARDS = [-10. -2. -0.02 10. ] + +MOTOR_POWER = 1 + +# RESULTS + +Average_reward = 3.37 + +Elapsed_time = 6025.17 +actual_step_time = 0.33 diff --git a/tasks/wander_4k.py b/tasks/wander_4k.py new file mode 100644 index 0000000..987b568 --- /dev/null +++ b/tasks/wander_4k.py @@ -0,0 +1,121 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "Wander_4096s_25a" +DESCRIPTION = "Wander avoiding obstacles. 6 lasers. 4 ranges each. 25 actions " +ROBOT = "Pioneer 3dx with 8-point laser" +ENVIRONMENT = "VREP_SIM: square 6x6m with obstacles" +ENVIRONMENT_DETAIL = "SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming" +AGENT_ELEMENTS = ["MOBILE_BASE", "DISTANCE_SENSOR"] +ENV_ELEMENTS = [] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR","GOAL","ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: +STEP_TIME = 1 # s +MOTOR_SPEED = 1 # rad/s (pioneer 3dx: 1 rad/s: ~ 0.1m/s) +RANGE_OBSTACLES = 0.80 # m +RANGE_DISPLACEMENT = 0.07 # m +RANGE_DAMAGE = 0.05 # m + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "laser_front": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_right": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_right": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_rear": np.linspace(0, RANGE_OBSTACLES, 4) +} +OUTPUT_VARIABLES = { + "left_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 5), + "right_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 5) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + v_left, v_right = actuator[0], actuator[1] + # backward action is replaced by double maximum speed + # actions with only one wheel backwards changed to no motion (duplicated) + if v_left < 0 and v_right < 0: + v_left = v_right = MOTOR_SPEED * 2 + elif v_left * v_right < 0: + v_left = v_right = 0 + robot.move_wheels(v_left, v_right) # left wheel, right_wheel speeds (rad/s) + + +# TASK DEFINITION: REWARDS ---------------------------------------------------- +REWARDS = np.array([-10.0, -2.0, -0.02, 10.0]) +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + distance_fl = robot.sensor["laser_front_left"] + distance_fr = robot.sensor["laser_front_right"] + distance_f = robot.sensor["laser_front"] + displacement = robot.mobilebase_displacement2d + + n_collisions = (int(distance_fl < RANGE_DAMAGE) + + int(distance_fr < RANGE_DAMAGE) + + int(distance_f < RANGE_DAMAGE)) + + r = REWARDS[2] + if n_collisions > 1: # big penalty + r = min(REWARDS) + elif n_collisions == 1: # small penalty + r = REWARDS[1] + elif displacement > RANGE_DISPLACEMENT: + r = max(REWARDS) + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/wander_4k_GIRAFF.py b/tasks/wander_4k_GIRAFF.py new file mode 100644 index 0000000..50b9ec3 --- /dev/null +++ b/tasks/wander_4k_GIRAFF.py @@ -0,0 +1,121 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "Wander_4096s_25a" +DESCRIPTION = "Wander avoiding obstacles. 6 lasers. 4 ranges each. 25 actions " +ROBOT = "Giraff" +ENVIRONMENT = "ROS" +ENVIRONMENT_DETAIL = "Trapezoid 1.5x1.0m" +AGENT_ELEMENTS = ["MOBILE_BASE", "DISTANCE_SENSOR"] +ENV_ELEMENTS = [] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR","GOAL","ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: (tuned for Giraff robot) +STEP_TIME = 1 # s +MOTOR_SPEED = 0.5 # rad/s (pioneer 3dx: 1 rad/s: ~ 0.1m/s) +RANGE_OBSTACLES = 0.25 # m +RANGE_DISPLACEMENT = 0.03 # m +RANGE_DAMAGE = 0.15 # m + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "laser_front": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_left": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_right": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_front_right": np.linspace(0, RANGE_OBSTACLES, 4), + "laser_rear": np.linspace(0, RANGE_OBSTACLES, 4) +} +OUTPUT_VARIABLES = { + "left_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 5), + "right_wheel": np.linspace(-MOTOR_SPEED, MOTOR_SPEED, 5) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + v_left, v_right = actuator[0], actuator[1] + # backward action is replaced by double maximum speed + # actions with only one wheel backwards changed to no motion (duplicated) + if v_left < 0 and v_right < 0: + v_left = v_right = MOTOR_SPEED * 2 + elif v_left * v_right < 0: + v_left = v_right = 0 + robot.move_wheels(v_left, v_right) # left wheel, right_wheel speeds (rad/s) + + +# TASK DEFINITION: REWARDS ---------------------------------------------------- +REWARDS = np.array([-10.0, -2.0, -0.02, 10.0]) +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + distance_fl = robot.sensor["laser_front_left"] + distance_fr = robot.sensor["laser_front_right"] + distance_f = robot.sensor["laser_front"] + displacement = robot.mobilebase_displacement2d + + n_collisions = (int(distance_fl < RANGE_DAMAGE) + + int(distance_fr < RANGE_DAMAGE) + + int(distance_f < RANGE_DAMAGE)) + + r = REWARDS[2] + if n_collisions > 1: # big penalty + r = min(REWARDS) + elif n_collisions == 1: # small penalty + r = REWARDS[1] + elif displacement > RANGE_DISPLACEMENT: + r = max(REWARDS) + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/wander_simple.py b/tasks/wander_simple.py new file mode 100644 index 0000000..a0dbdd9 --- /dev/null +++ b/tasks/wander_simple.py @@ -0,0 +1,110 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "Wander_4s_4a" +DESCRIPTION = "Wander avoiding obstacles. 2 laser. 2 ranges each" +ROBOT = "Pioneer 3dx with 8-point laser" +ENVIRONMENT = "VREP_SIM: square 2x2m" +ENVIRONMENT_DETAIL = "SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming" +AGENT_ELEMENTS = ["MOBILE_BASE", "DISTANCE_SENSOR"] +ENV_ELEMENTS = [] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR","GOAL","ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: +STEP_TIME = 1 # s +MOTOR_SPEED = 1 # rad/s (pioneer 3dx: 1 rad/s: ~ 0.1m/s) +RANGE_OBSTACLES = 0.20 # m +RANGE_DISPLACEMENT = 0.07 # m +RANGE_DAMAGE = 0.05 # m + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "laser_front_left": (0, RANGE_OBSTACLES), + "laser_front_right": (0, RANGE_OBSTACLES) +} +OUTPUT_VARIABLES = { + "left_wheel": (-MOTOR_SPEED, MOTOR_SPEED), + "right_wheel": (-MOTOR_SPEED, MOTOR_SPEED) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): # two differential drive wheels template + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + # backward action is replaced by no motion: + v_left, v_right = actuator[0], actuator[1] + if v_left < 0 and v_right < 0: + v_left = 0 + v_right = 0 + robot.move_wheels(v_left, v_right) # left wheel, right_wheel speeds (rad/s) + + +# Rewards +REWARDS = np.array([-10.0, -2.0, -0.02, 10.0]) +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + distance_fl = robot.sensor["laser_front_left"] + distance_fr = robot.sensor["laser_front_right"] + displacement = robot.mobilebase_displacement2d + + r = REWARDS[2] + if distance_fl < RANGE_DAMAGE and distance_fr < RANGE_DAMAGE: + r = min(REWARDS) + elif distance_fl < 0.1 or distance_fr < 0.1: + r = REWARDS[1] + elif displacement > RANGE_DISPLACEMENT: + r = max(REWARDS) + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/wander_simple_GIRAFF.py b/tasks/wander_simple_GIRAFF.py new file mode 100644 index 0000000..e547bcb --- /dev/null +++ b/tasks/wander_simple_GIRAFF.py @@ -0,0 +1,110 @@ +# -*- coding: utf-8 -*- +# +-----------------------------------------------+ +# | RL-ROBOT. Reinforcement Learning for Robotics | +# | Angel Martinez-Tenor | +# | MAPIR. University of Malaga. 2016 | +# +-----------------------------------------------+ +""" Task template v1.1 """ + +import numpy as np + +import agent +import robot + +# Task Parameters: +NAME = "Wander_4s_4a" +DESCRIPTION = "Wander avoiding obstacles. 2 laser. 2 ranges each" +ROBOT = "Giraff" +ENVIRONMENT = "ROS" +ENVIRONMENT_DETAIL = "Trapezoid 1.5x1.0m" +AGENT_ELEMENTS = ["MOBILE_BASE", "DISTANCE_SENSOR"] +ENV_ELEMENTS = [] +# AGENT_ELEMENTS: "MOBILE_BASE","DISTANCE_SENSOR","GOAL","ARM" +# ENV_ELEMENTS: "GOAL_OBJECT" + +# Physical Parameters: (tuned for Giraff robot) +STEP_TIME = 1 # s +MOTOR_SPEED = 0.5 # rad/s +RANGE_OBSTACLES = 0.20 # m +RANGE_DISPLACEMENT = 0.03 # m +RANGE_DAMAGE = 0.15 # m + +# STATES. Input variables: +# Dictionary -> "variable name": (discrete intervals). Example: +# "X": (0, 5) 2 ranges: s(X)=0 if X<=5, =1 if X>5. +# "X": (0, 2, 4) 3 ranges: s(X)=0 if X<=2, =1 if X<4, =2 if X>4 +# For multiple linear intervals we suggest np.linspace +INPUT_VARIABLES = { + "laser_front_left": (0, RANGE_OBSTACLES), + "laser_front_right": (0, RANGE_OBSTACLES) +} +OUTPUT_VARIABLES = { + "left_wheel": (-MOTOR_SPEED, MOTOR_SPEED), + "right_wheel": (-MOTOR_SPEED, MOTOR_SPEED) +} +INITIAL_STATE = 0 # (usually overwritten by the fist observation) +INITIAL_POLICY = 0 + + +def execute_action(actuator): # two differential drive wheels template + """ Send the commands to the actuators of the robot. + input: vector of actuator values: e.g. [2.0,-2.0] rad/s """ + assert len(actuator) == len(out_data), " Check output variables" + # backward action is replaced by no motion: + v_left, v_right = actuator[0], actuator[1] + if v_left < 0 and v_right < 0: + v_left = 0 + v_right = 0 + robot.move_wheels(v_left, v_right) # left wheel, right_wheel speeds (rad/s) + + +# Rewards +REWARDS = np.array([-10.0, -2.0, -0.02, 10.0]) +goal_reached = False + + +def get_reward(): # abstract s,a,sp pointless here + """ Return the reward from s,a,sp or the environment (recommended)""" + # Sensors values already updated in robot.py when the state was observed + distance_fl = robot.sensor["laser_front_left"] + distance_fr = robot.sensor["laser_front_right"] + displacement = robot.mobilebase_displacement2d + + r = REWARDS[2] + if distance_fl < RANGE_DAMAGE and distance_fr < RANGE_DAMAGE: + r = min(REWARDS) + elif distance_fl < 0.1 or distance_fr < 0.1: + r = REWARDS[1] + elif displacement > RANGE_DISPLACEMENT: + r = max(REWARDS) + return r + + +# ------------------------------------------------------------------------------ +def get_input_data(): # -- no modification needed -- + """ Ask for sensory data to the robot and returns a vector with the values. + Relate Input Variables with robot sensors """ + global in_data + for idx, item in enumerate(in_names): + in_data[idx] = robot.sensor[item] + return in_data + + +def setup(): + """ task module setup is performed in agent """ + agent.setup_task() + + +n_inputs = int +in_values = [None] +in_names = [None] +in_sizes = [int] +n_states = int +in_data = [None] + +n_outputs = int +out_values = [None] +out_names = [None] +out_sizes = [int] +n_actions = int +out_data = [None] diff --git a/tasks/wander_simple_model/wander_simple_model.npz b/tasks/wander_simple_model/wander_simple_model.npz new file mode 100644 index 0000000..a5d7684 Binary files /dev/null and b/tasks/wander_simple_model/wander_simple_model.npz differ diff --git a/tasks/wander_simple_model/wander_simple_model_SASR_step b/tasks/wander_simple_model/wander_simple_model_SASR_step new file mode 100644 index 0000000..06c4c29 Binary files /dev/null and b/tasks/wander_simple_model/wander_simple_model_SASR_step differ diff --git a/tasks/wander_simple_model/wander_simple_model_log.py b/tasks/wander_simple_model/wander_simple_model_log.py new file mode 100644 index 0000000..00c1fd7 --- /dev/null +++ b/tasks/wander_simple_model/wander_simple_model_log.py @@ -0,0 +1,45 @@ +""" EXPERIMENT LOG """ + +# PARAMETERS +import numpy as np + +NAME = 'Wander_Maze2x2_4s4a' +DESCRIPTION = 'Wander avoiding obstacles. 2 Distance Ranges with 2 Lasers' +ALGORITHM_NAME = 'LE_TOSL' +EXPLORATION_STRATEGY = 'SOFT' +ROBOT = 'Pioneer 3dx with 8-point laser' +LEARN_FROM_MODEL = 'False' +ENVIRONMENT = 'VREP_SIM: square 2x2' +ENVIRONMENT_DETAIL = 'SpeedX3. Threaded Render. Sens:Buffer, Act:Streaming' +AGENT_ELEMENTS = ['MOBILE_BASE', 'DISTANCE_SENSOR'] +ENV_ELEMENTS = [] +N_STATES = 4 +N_ACTIONS = 4 +GAMMA = 0.9 +INITIAL_ALPHA = 0.1 +STEP_TIME = 0.333333333333 +INITIAL_POLICY = 0 + +steps = 15106 + +# REWARDS = [-10. -2. -0.02 10. ] + +MOTOR_POWER = 1 + +# RESULTS +Average_reward = 0.00 +Elapsed_time = 0.00 +actual_step_time = 0.00 +policy = np.array([2.00, 2.00, 1.00, 3.00]) + +V = np.array([54.82, 62.28, 28.97, 80.24]) + +Q = np.array([[-2.39, 0.55, 54.82, 4.66], + [2.10, -3.55, 62.28, 12.71], + [16.16, 28.97, 0.60, 25.85], + [11.06, 8.32, 7.87, 80.24]]) + +Q_count = np.array([[22, 32, 300, 27], + [11, 13, 2850, 13], + [18, 34, 17, 29], + [10, 10, 8, 11711]]) diff --git a/tasks/z_files.py b/tasks/z_files.py new file mode 100644 index 0000000..b8420ee --- /dev/null +++ b/tasks/z_files.py @@ -0,0 +1,25 @@ +""" File Management """ + +import os +import re + + +def purge(pattern): + """ remove files """ + for f in os.listdir("."): + if re.search(pattern, f): + os.remove(os.path.join(".", f)) + + +def rename(pattern, new_string): + """ rename files """ + i = 0 + + for f in os.listdir("."): + if re.search(pattern, f): + os.rename(f, f.replace(pattern, new_string)) + i += 1 + print(str(i) + " files renamed") + + +rename("Arm_cube_table_4096s9a__LE_TOSL__SOFT__T3", "3d_arm_table_4096s_9a") diff --git a/vrep.py b/vrep.py new file mode 100755 index 0000000..8e23f72 --- /dev/null +++ b/vrep.py @@ -0,0 +1,1481 @@ +# This file is part of the REMOTE API +# +# Copyright 2006-2016 Coppelia Robotics GmbH. All rights reserved. +# marc@coppeliarobotics.com +# www.coppeliarobotics.com +# +# The REMOTE API is licensed under the terms of GNU GPL: +# +# ------------------------------------------------------------------- +# The REMOTE API is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# THE REMOTE API IS DISTRIBUTED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED +# WARRANTY. THE USER WILL USE IT AT HIS/HER OWN RISK. THE ORIGINAL +# AUTHORS AND COPPELIA ROBOTICS GMBH WILL NOT BE LIABLE FOR DATA LOSS, +# DAMAGES, LOSS OF PROFITS OR ANY OTHER KIND OF LOSS WHILE USING OR +# MISUSING THIS SOFTWARE. +# +# See the GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with the REMOTE API. If not, see . +# ------------------------------------------------------------------- +# +# This file was automatically created for V-REP release V3.3.2 on August 29th 2016 + +import platform +import struct +import sys +import ctypes as ct +from vrepConst import * + +#load library +libsimx = None +try: + if platform.system() =='cli': + libsimx = ct.CDLL("./remoteApi.dll") + elif platform.system() =='Windows': + libsimx = ct.CDLL("./remoteApi.dll") + elif platform.system() == 'Darwin': + libsimx = ct.CDLL("./remoteApi.dylib") + else: + libsimx = ct.CDLL("./remoteApi.so") +except: + print ('----------------------------------------------------') + print ('The remoteApi library could not be loaded. Make sure') + print ('it is located in the same folder as "vrep.py", or') + print ('appropriately adjust the file "vrep.py"') + print ('----------------------------------------------------') + print ('') + +#ctypes wrapper prototypes +c_GetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointPosition", libsimx)) +c_SetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointPosition", libsimx)) +c_GetJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointMatrix", libsimx)) +c_SetSphericalJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetSphericalJointMatrix", libsimx)) +c_SetJointTargetVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetVelocity", libsimx)) +c_SetJointTargetPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetPosition", libsimx)) +c_GetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointForce", libsimx)) +c_SetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointForce", libsimx)) +c_ReadForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadForceSensor", libsimx)) +c_BreakForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxBreakForceSensor", libsimx)) +c_ReadVisionSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxReadVisionSensor", libsimx)) +c_GetObjectHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectHandle", libsimx)) +c_GetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_byte)), ct.c_ubyte, ct.c_int32)(("simxGetVisionSensorImage", libsimx)) +c_SetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_byte), ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetVisionSensorImage", libsimx)) +c_GetVisionSensorDepthBuffer= ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.c_int32)(("simxGetVisionSensorDepthBuffer", libsimx)) +c_GetObjectChild = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectChild", libsimx)) +c_GetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectParent", libsimx)) +c_ReadProximitySensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadProximitySensor", libsimx)) +c_LoadModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.c_int32)(("simxLoadModel", libsimx)) +c_LoadUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxLoadUI", libsimx)) +c_LoadScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.c_int32)(("simxLoadScene", libsimx)) +c_StartSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStartSimulation", libsimx)) +c_PauseSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxPauseSimulation", libsimx)) +c_StopSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStopSimulation", libsimx)) +c_GetUIHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIHandle", libsimx)) +c_GetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUISlider", libsimx)) +c_SetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUISlider", libsimx)) +c_GetUIEventButton = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIEventButton", libsimx)) +c_GetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIButtonProperty", libsimx)) +c_SetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUIButtonProperty", libsimx)) +c_AddStatusbarMessage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAddStatusbarMessage", libsimx)) +c_AuxiliaryConsoleOpen = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.c_int32)(("simxAuxiliaryConsoleOpen", libsimx)) +c_AuxiliaryConsoleClose = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxAuxiliaryConsoleClose", libsimx)) +c_AuxiliaryConsolePrint = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAuxiliaryConsolePrint", libsimx)) +c_AuxiliaryConsoleShow = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxAuxiliaryConsoleShow", libsimx)) +c_GetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectOrientation", libsimx)) +c_GetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectPosition", libsimx)) +c_SetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectOrientation", libsimx)) +c_SetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectPosition", libsimx)) +c_SetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetObjectParent", libsimx)) +c_SetUIButtonLabel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32)(("simxSetUIButtonLabel", libsimx)) +c_GetLastErrors = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetLastErrors", libsimx)) +c_GetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetArrayParameter", libsimx)) +c_SetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetArrayParameter", libsimx)) +c_GetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxGetBooleanParameter", libsimx)) +c_SetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetBooleanParameter", libsimx)) +c_GetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerParameter", libsimx)) +c_SetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetIntegerParameter", libsimx)) +c_GetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatingParameter", libsimx)) +c_SetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetFloatingParameter", libsimx)) +c_GetStringParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetStringParameter", libsimx)) +c_GetCollisionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollisionHandle", libsimx)) +c_GetDistanceHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDistanceHandle", libsimx)) +c_GetCollectionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollectionHandle", libsimx)) +c_ReadCollision = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxReadCollision", libsimx)) +c_ReadDistance = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxReadDistance", libsimx)) +c_RemoveObject = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveObject", libsimx)) +c_RemoveModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveModel", libsimx)) +c_RemoveUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveUI", libsimx)) +c_CloseScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxCloseScene", libsimx)) +c_GetObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxGetObjects", libsimx)) +c_DisplayDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxDisplayDialog", libsimx)) +c_EndDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxEndDialog", libsimx)) +c_GetDialogInput = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetDialogInput", libsimx)) +c_GetDialogResult = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDialogResult", libsimx)) +c_CopyPasteObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCopyPasteObjects", libsimx)) +c_GetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectSelection", libsimx)) +c_SetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.c_int32)(("simxSetObjectSelection", libsimx)) +c_ClearFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearFloatSignal", libsimx)) +c_ClearIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearIntegerSignal", libsimx)) +c_ClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearStringSignal", libsimx)) +c_GetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatSignal", libsimx)) +c_GetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerSignal", libsimx)) +c_GetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetStringSignal", libsimx)) +c_SetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_float, ct.c_int32)(("simxSetFloatSignal", libsimx)) +c_SetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxSetIntegerSignal", libsimx)) +c_SetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxSetStringSignal", libsimx)) +c_AppendStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxAppendStringSignal", libsimx)) +c_WriteStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxWriteStringStream", libsimx)) +c_GetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectFloatParameter", libsimx)) +c_SetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetObjectFloatParameter", libsimx)) +c_GetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectIntParameter", libsimx)) +c_SetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetObjectIntParameter", libsimx)) +c_GetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetModelProperty", libsimx)) +c_SetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetModelProperty", libsimx)) +c_Start = ct.CFUNCTYPE(ct.c_int32,ct.POINTER(ct.c_char), ct.c_int32, ct.c_ubyte, ct.c_ubyte, ct.c_int32, ct.c_int32)(("simxStart", libsimx)) +c_Finish = ct.CFUNCTYPE(None, ct.c_int32)(("simxFinish", libsimx)) +c_GetPingTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetPingTime", libsimx)) +c_GetLastCmdTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetLastCmdTime", libsimx)) +c_SynchronousTrigger = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxSynchronousTrigger", libsimx)) +c_Synchronous = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxSynchronous", libsimx)) +c_PauseCommunication = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxPauseCommunication", libsimx)) +c_GetInMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetInMessageInfo", libsimx)) +c_GetOutMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetOutMessageInfo", libsimx)) +c_GetConnectionId = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetConnectionId", libsimx)) +c_CreateBuffer = ct.CFUNCTYPE(ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxCreateBuffer", libsimx)) +c_ReleaseBuffer = ct.CFUNCTYPE(None, ct.c_void_p)(("simxReleaseBuffer", libsimx)) +c_TransferFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxTransferFile", libsimx)) +c_EraseFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxEraseFile", libsimx)) +c_GetAndClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetAndClearStringSignal", libsimx)) +c_ReadStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxReadStringStream", libsimx)) +c_CreateDummy = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_float, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCreateDummy", libsimx)) +c_Query = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxQuery", libsimx)) +c_GetObjectGroupData = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetObjectGroupData", libsimx)) +c_GetObjectVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectVelocity", libsimx)) +c_CallScriptFunction = ct.CFUNCTYPE(ct.c_int32,ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_int32),ct.c_int32,ct.POINTER(ct.c_float),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_ubyte),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_ubyte)),ct.c_int32)(("simxCallScriptFunction", libsimx)) + +#API functions +def simxGetJointPosition(clientID, jointHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + position = ct.c_float() + return c_GetJointPosition(clientID, jointHandle, ct.byref(position), operationMode), position.value + +def simxSetJointPosition(clientID, jointHandle, position, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetJointPosition(clientID, jointHandle, position, operationMode) + +def simxGetJointMatrix(clientID, jointHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + matrix = (ct.c_float*12)() + ret = c_GetJointMatrix(clientID, jointHandle, matrix, operationMode) + arr = [] + for i in range(12): + arr.append(matrix[i]) + return ret, arr + +def simxSetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + matrix = (ct.c_float*12)(*matrix) + return c_SetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode) + +def simxSetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode) + +def simxSetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode) + +def simxJointGetForce(clientID, jointHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + force = ct.c_float() + return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value + +def simxGetJointForce(clientID, jointHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + force = ct.c_float() + return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value + +def simxSetJointForce(clientID, jointHandle, force, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + return c_SetJointForce(clientID, jointHandle, force, operationMode) + +def simxReadForceSensor(clientID, forceSensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + state = ct.c_ubyte() + forceVector = (ct.c_float*3)() + torqueVector = (ct.c_float*3)() + ret = c_ReadForceSensor(clientID, forceSensorHandle, ct.byref(state), forceVector, torqueVector, operationMode) + arr1 = [] + for i in range(3): + arr1.append(forceVector[i]) + arr2 = [] + for i in range(3): + arr2.append(torqueVector[i]) + #if sys.version_info[0] == 3: + # state=state.value + #else: + # state=ord(state.value) + return ret, state.value, arr1, arr2 + +def simxBreakForceSensor(clientID, forceSensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + return c_BreakForceSensor(clientID, forceSensorHandle, operationMode) + +def simxReadVisionSensor(clientID, sensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + detectionState = ct.c_ubyte() + auxValues = ct.POINTER(ct.c_float)() + auxValuesCount = ct.POINTER(ct.c_int)() + ret = c_ReadVisionSensor(clientID, sensorHandle, ct.byref(detectionState), ct.byref(auxValues), ct.byref(auxValuesCount), operationMode) + + auxValues2 = [] + if ret == 0: + s = 0 + for i in range(auxValuesCount[0]): + auxValues2.append(auxValues[s:s+auxValuesCount[i+1]]) + s += auxValuesCount[i+1] + + #free C buffers + c_ReleaseBuffer(auxValues) + c_ReleaseBuffer(auxValuesCount) + + return ret, bool(detectionState.value!=0), auxValues2 + +def simxGetObjectHandle(clientID, objectName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(objectName) is str): + objectName=objectName.encode('utf-8') + return c_GetObjectHandle(clientID, objectName, ct.byref(handle), operationMode), handle.value + +def simxGetVisionSensorImage(clientID, sensorHandle, options, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + resolution = (ct.c_int*2)() + c_image = ct.POINTER(ct.c_byte)() + bytesPerPixel = 3 + if (options and 1) != 0: + bytesPerPixel = 1 + ret = c_GetVisionSensorImage(clientID, sensorHandle, resolution, ct.byref(c_image), options, operationMode) + + reso = [] + image = [] + if (ret == 0): + image = [None]*resolution[0]*resolution[1]*bytesPerPixel + for i in range(resolution[0] * resolution[1] * bytesPerPixel): + image[i] = c_image[i] + for i in range(2): + reso.append(resolution[i]) + return ret, reso, image + +def simxSetVisionSensorImage(clientID, sensorHandle, image, options, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + size = len(image) + image_bytes = (ct.c_byte*size)(*image) + return c_SetVisionSensorImage(clientID, sensorHandle, image_bytes, size, options, operationMode) + +def simxGetVisionSensorDepthBuffer(clientID, sensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + c_buffer = ct.POINTER(ct.c_float)() + resolution = (ct.c_int*2)() + ret = c_GetVisionSensorDepthBuffer(clientID, sensorHandle, resolution, ct.byref(c_buffer), operationMode) + reso = [] + buffer = [] + if (ret == 0): + buffer = [None]*resolution[0]*resolution[1] + for i in range(resolution[0] * resolution[1]): + buffer[i] = c_buffer[i] + for i in range(2): + reso.append(resolution[i]) + return ret, reso, buffer + +def simxGetObjectChild(clientID, parentObjectHandle, childIndex, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + childObjectHandle = ct.c_int() + return c_GetObjectChild(clientID, parentObjectHandle, childIndex, ct.byref(childObjectHandle), operationMode), childObjectHandle.value + +def simxGetObjectParent(clientID, childObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + parentObjectHandle = ct.c_int() + return c_GetObjectParent(clientID, childObjectHandle, ct.byref(parentObjectHandle), operationMode), parentObjectHandle.value + +def simxReadProximitySensor(clientID, sensorHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + detectionState = ct.c_ubyte() + detectedObjectHandle = ct.c_int() + detectedPoint = (ct.c_float*3)() + detectedSurfaceNormalVector = (ct.c_float*3)() + ret = c_ReadProximitySensor(clientID, sensorHandle, ct.byref(detectionState), detectedPoint, ct.byref(detectedObjectHandle), detectedSurfaceNormalVector, operationMode) + arr1 = [] + for i in range(3): + arr1.append(detectedPoint[i]) + arr2 = [] + for i in range(3): + arr2.append(detectedSurfaceNormalVector[i]) + return ret, bool(detectionState.value!=0), arr1, detectedObjectHandle.value, arr2 + +def simxLoadModel(clientID, modelPathAndName, options, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + baseHandle = ct.c_int() + if (sys.version_info[0] == 3) and (type(modelPathAndName) is str): + modelPathAndName=modelPathAndName.encode('utf-8') + return c_LoadModel(clientID, modelPathAndName, options, ct.byref(baseHandle), operationMode), baseHandle.value + +def simxLoadUI(clientID, uiPathAndName, options, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + count = ct.c_int() + uiHandles = ct.POINTER(ct.c_int)() + if (sys.version_info[0] == 3) and (type(uiPathAndName) is str): + uiPathAndName=uiPathAndName.encode('utf-8') + ret = c_LoadUI(clientID, uiPathAndName, options, ct.byref(count), ct.byref(uiHandles), operationMode) + + handles = [] + if ret == 0: + for i in range(count.value): + handles.append(uiHandles[i]) + #free C buffers + c_ReleaseBuffer(uiHandles) + + return ret, handles + +def simxLoadScene(clientID, scenePathAndName, options, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(scenePathAndName) is str): + scenePathAndName=scenePathAndName.encode('utf-8') + return c_LoadScene(clientID, scenePathAndName, options, operationMode) + +def simxStartSimulation(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_StartSimulation(clientID, operationMode) + +def simxPauseSimulation(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_PauseSimulation(clientID, operationMode) + +def simxStopSimulation(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_StopSimulation(clientID, operationMode) + +def simxGetUIHandle(clientID, uiName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(uiName) is str): + uiName=uiName.encode('utf-8') + return c_GetUIHandle(clientID, uiName, ct.byref(handle), operationMode), handle.value + +def simxGetUISlider(clientID, uiHandle, uiButtonID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + position = ct.c_int() + return c_GetUISlider(clientID, uiHandle, uiButtonID, ct.byref(position), operationMode), position.value + +def simxSetUISlider(clientID, uiHandle, uiButtonID, position, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetUISlider(clientID, uiHandle, uiButtonID, position, operationMode) + +def simxGetUIEventButton(clientID, uiHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + uiEventButtonID = ct.c_int() + auxValues = (ct.c_int*2)() + ret = c_GetUIEventButton(clientID, uiHandle, ct.byref(uiEventButtonID), auxValues, operationMode) + arr = [] + for i in range(2): + arr.append(auxValues[i]) + return ret, uiEventButtonID.value, arr + +def simxGetUIButtonProperty(clientID, uiHandle, uiButtonID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + prop = ct.c_int() + return c_GetUIButtonProperty(clientID, uiHandle, uiButtonID, ct.byref(prop), operationMode), prop.value + +def simxSetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode) + +def simxAddStatusbarMessage(clientID, message, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(message) is str): + message=message.encode('utf-8') + return c_AddStatusbarMessage(clientID, message, operationMode) + +def simxAuxiliaryConsoleOpen(clientID, title, maxLines, mode, position, size, textColor, backgroundColor, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + consoleHandle = ct.c_int() + if (sys.version_info[0] == 3) and (type(title) is str): + title=title.encode('utf-8') + if position != None: + c_position = (ct.c_int*2)(*position) + else: + c_position = None + if size != None: + c_size = (ct.c_int*2)(*size) + else: + c_size = None + if textColor != None: + c_textColor = (ct.c_float*3)(*textColor) + else: + c_textColor = None + if backgroundColor != None: + c_backgroundColor = (ct.c_float*3)(*backgroundColor) + else: + c_backgroundColor = None + return c_AuxiliaryConsoleOpen(clientID, title, maxLines, mode, c_position, c_size, c_textColor, c_backgroundColor, ct.byref(consoleHandle), operationMode), consoleHandle.value + +def simxAuxiliaryConsoleClose(clientID, consoleHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_AuxiliaryConsoleClose(clientID, consoleHandle, operationMode) + +def simxAuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(txt) is str): + txt=txt.encode('utf-8') + return c_AuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode) + +def simxAuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_AuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode) + +def simxGetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + eulerAngles = (ct.c_float*3)() + ret = c_GetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode) + arr = [] + for i in range(3): + arr.append(eulerAngles[i]) + return ret, arr + +def simxGetObjectPosition(clientID, objectHandle, relativeToObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + position = (ct.c_float*3)() + ret = c_GetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode) + arr = [] + for i in range(3): + arr.append(position[i]) + return ret, arr + +def simxSetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + angles = (ct.c_float*3)(*eulerAngles) + return c_SetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, angles, operationMode) + +def simxSetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + c_position = (ct.c_float*3)(*position) + return c_SetObjectPosition(clientID, objectHandle, relativeToObjectHandle, c_position, operationMode) + +def simxSetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode) + +def simxSetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if sys.version_info[0] == 3: + if type(upStateLabel) is str: + upStateLabel=upStateLabel.encode('utf-8') + if type(downStateLabel) is str: + downStateLabel=downStateLabel.encode('utf-8') + return c_SetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode) + +def simxGetLastErrors(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + errors =[] + errorCnt = ct.c_int() + errorStrings = ct.POINTER(ct.c_char)() + ret = c_GetLastErrors(clientID, ct.byref(errorCnt), ct.byref(errorStrings), operationMode) + if ret == 0: + s = 0 + for i in range(errorCnt.value): + a = bytearray() + while errorStrings[s] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(errorStrings[s],'big')) + else: + a.append(errorStrings[s]) + s += 1 + s += 1 #skip null + if sys.version_info[0] == 3: + errors.append(str(a,'utf-8')) + else: + errors.append(str(a)) + + return ret, errors + +def simxGetArrayParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + paramValues = (ct.c_float*3)() + ret = c_GetArrayParameter(clientID, paramIdentifier, paramValues, operationMode) + arr = [] + for i in range(3): + arr.append(paramValues[i]) + return ret, arr + +def simxSetArrayParameter(clientID, paramIdentifier, paramValues, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + c_paramValues = (ct.c_float*3)(*paramValues) + return c_SetArrayParameter(clientID, paramIdentifier, c_paramValues, operationMode) + +def simxGetBooleanParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + paramValue = ct.c_ubyte() + return c_GetBooleanParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), bool(paramValue.value!=0) + +def simxSetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode) + +def simxGetIntegerParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + paramValue = ct.c_int() + return c_GetIntegerParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value + +def simxSetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode) + +def simxGetFloatingParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + paramValue = ct.c_float() + return c_GetFloatingParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value + +def simxSetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode) + +def simxGetStringParameter(clientID, paramIdentifier, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + paramValue = ct.POINTER(ct.c_char)() + ret = c_GetStringParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode) + + a = bytearray() + if ret == 0: + i = 0 + while paramValue[i] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(paramValue[i],'big')) + else: + a.append(paramValue[i]) + i=i+1 + if sys.version_info[0] == 3: + a=str(a,'utf-8') + else: + a=str(a) + return ret, a + +def simxGetCollisionHandle(clientID, collisionObjectName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(collisionObjectName) is str): + collisionObjectName=collisionObjectName.encode('utf-8') + return c_GetCollisionHandle(clientID, collisionObjectName, ct.byref(handle), operationMode), handle.value + +def simxGetCollectionHandle(clientID, collectionName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(collectionName) is str): + collectionName=collectionName.encode('utf-8') + return c_GetCollectionHandle(clientID, collectionName, ct.byref(handle), operationMode), handle.value + +def simxGetDistanceHandle(clientID, distanceObjectName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + handle = ct.c_int() + if (sys.version_info[0] == 3) and (type(distanceObjectName) is str): + distanceObjectName=distanceObjectName.encode('utf-8') + return c_GetDistanceHandle(clientID, distanceObjectName, ct.byref(handle), operationMode), handle.value + +def simxReadCollision(clientID, collisionObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + collisionState = ct.c_ubyte() + return c_ReadCollision(clientID, collisionObjectHandle, ct.byref(collisionState), operationMode), bool(collisionState.value!=0) + +def simxReadDistance(clientID, distanceObjectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + minimumDistance = ct.c_float() + return c_ReadDistance(clientID, distanceObjectHandle, ct.byref(minimumDistance), operationMode), minimumDistance.value + +def simxRemoveObject(clientID, objectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_RemoveObject(clientID, objectHandle, operationMode) + +def simxRemoveModel(clientID, objectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_RemoveModel(clientID, objectHandle, operationMode) + +def simxRemoveUI(clientID, uiHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_RemoveUI(clientID, uiHandle, operationMode) + +def simxCloseScene(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_CloseScene(clientID, operationMode) + +def simxGetObjects(clientID, objectType, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + objectCount = ct.c_int() + objectHandles = ct.POINTER(ct.c_int)() + + ret = c_GetObjects(clientID, objectType, ct.byref(objectCount), ct.byref(objectHandles), operationMode) + handles = [] + if ret == 0: + for i in range(objectCount.value): + handles.append(objectHandles[i]) + + return ret, handles + + +def simxDisplayDialog(clientID, titleText, mainText, dialogType, initialText, titleColors, dialogColors, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + if titleColors != None: + c_titleColors = (ct.c_float*6)(*titleColors) + else: + c_titleColors = None + if dialogColors != None: + c_dialogColors = (ct.c_float*6)(*dialogColors) + else: + c_dialogColors = None + + c_dialogHandle = ct.c_int() + c_uiHandle = ct.c_int() + if sys.version_info[0] == 3: + if type(titleText) is str: + titleText=titleText.encode('utf-8') + if type(mainText) is str: + mainText=mainText.encode('utf-8') + if type(initialText) is str: + initialText=initialText.encode('utf-8') + return c_DisplayDialog(clientID, titleText, mainText, dialogType, initialText, c_titleColors, c_dialogColors, ct.byref(c_dialogHandle), ct.byref(c_uiHandle), operationMode), c_dialogHandle.value, c_uiHandle.value + +def simxEndDialog(clientID, dialogHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_EndDialog(clientID, dialogHandle, operationMode) + +def simxGetDialogInput(clientID, dialogHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + inputText = ct.POINTER(ct.c_char)() + ret = c_GetDialogInput(clientID, dialogHandle, ct.byref(inputText), operationMode) + + a = bytearray() + if ret == 0: + i = 0 + while inputText[i] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(inputText[i],'big')) + else: + a.append(inputText[i]) + i = i+1 + + if sys.version_info[0] == 3: + a=str(a,'utf-8') + else: + a=str(a) + return ret, a + + +def simxGetDialogResult(clientID, dialogHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + result = ct.c_int() + return c_GetDialogResult(clientID, dialogHandle, ct.byref(result), operationMode), result.value + +def simxCopyPasteObjects(clientID, objectHandles, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) + c_objectHandles = ct.cast(c_objectHandles,ct.POINTER(ct.c_int)) # IronPython needs this + newObjectCount = ct.c_int() + newObjectHandles = ct.POINTER(ct.c_int)() + ret = c_CopyPasteObjects(clientID, c_objectHandles, len(objectHandles), ct.byref(newObjectHandles), ct.byref(newObjectCount), operationMode) + + newobj = [] + if ret == 0: + for i in range(newObjectCount.value): + newobj.append(newObjectHandles[i]) + + return ret, newobj + + +def simxGetObjectSelection(clientID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + objectCount = ct.c_int() + objectHandles = ct.POINTER(ct.c_int)() + ret = c_GetObjectSelection(clientID, ct.byref(objectHandles), ct.byref(objectCount), operationMode) + + newobj = [] + if ret == 0: + for i in range(objectCount.value): + newobj.append(objectHandles[i]) + + return ret, newobj + + + +def simxSetObjectSelection(clientID, objectHandles, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) + return c_SetObjectSelection(clientID, c_objectHandles, len(objectHandles), operationMode) + +def simxClearFloatSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_ClearFloatSignal(clientID, signalName, operationMode) + +def simxClearIntegerSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_ClearIntegerSignal(clientID, signalName, operationMode) + +def simxClearStringSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_ClearStringSignal(clientID, signalName, operationMode) + +def simxGetFloatSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + signalValue = ct.c_float() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_GetFloatSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value + +def simxGetIntegerSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + signalValue = ct.c_int() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_GetIntegerSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value + +def simxGetStringSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + signalLength = ct.c_int(); + signalValue = ct.POINTER(ct.c_ubyte)() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + ret = c_GetStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) + + a = bytearray() + if ret == 0: + for i in range(signalLength.value): + a.append(signalValue[i]) + if sys.version_info[0] != 3: + a=str(a) + + return ret, a + +def simxGetAndClearStringSignal(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + signalLength = ct.c_int(); + signalValue = ct.POINTER(ct.c_ubyte)() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + ret = c_GetAndClearStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) + + a = bytearray() + if ret == 0: + for i in range(signalLength.value): + a.append(signalValue[i]) + if sys.version_info[0] != 3: + a=str(a) + + return ret, a + +def simxReadStringStream(clientID, signalName, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + signalLength = ct.c_int(); + signalValue = ct.POINTER(ct.c_ubyte)() + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + ret = c_ReadStringStream(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) + + a = bytearray() + if ret == 0: + for i in range(signalLength.value): + a.append(signalValue[i]) + if sys.version_info[0] != 3: + a=str(a) + + return ret, a + +def simxSetFloatSignal(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_SetFloatSignal(clientID, signalName, signalValue, operationMode) + +def simxSetIntegerSignal(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(signalName) is str): + signalName=signalName.encode('utf-8') + return c_SetIntegerSignal(clientID, signalName, signalValue, operationMode) + +def simxSetStringSignal(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + sigV=signalValue + if sys.version_info[0] == 3: + if type(signalName) is str: + signalName=signalName.encode('utf-8') + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=signalValue.encode('utf-8') + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + else: + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=bytearray(signalValue) + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + return c_SetStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) + +def simxAppendStringSignal(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + sigV=signalValue + if sys.version_info[0] == 3: + if type(signalName) is str: + signalName=signalName.encode('utf-8') + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=signalValue.encode('utf-8') + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + else: + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=bytearray(signalValue) + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + return c_AppendStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) + +def simxWriteStringStream(clientID, signalName, signalValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + sigV=signalValue + if sys.version_info[0] == 3: + if type(signalName) is str: + signalName=signalName.encode('utf-8') + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=signalValue.encode('utf-8') + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + else: + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=bytearray(signalValue) + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + return c_WriteStringStream(clientID, signalName, sigV, len(signalValue), operationMode) + +def simxGetObjectFloatParameter(clientID, objectHandle, parameterID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + parameterValue = ct.c_float() + return c_GetObjectFloatParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value + +def simxSetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) + +def simxGetObjectIntParameter(clientID, objectHandle, parameterID, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + parameterValue = ct.c_int() + return c_GetObjectIntParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value + +def simxSetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) + +def simxGetModelProperty(clientID, objectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + prop = ct.c_int() + return c_GetModelProperty(clientID, objectHandle, ct.byref(prop), operationMode), prop.value + +def simxSetModelProperty(clientID, objectHandle, prop, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SetModelProperty(clientID, objectHandle, prop, operationMode) + +def simxStart(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(connectionAddress) is str): + connectionAddress=connectionAddress.encode('utf-8') + return c_Start(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs) + +def simxFinish(clientID): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_Finish(clientID) + +def simxGetPingTime(clientID): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + pingTime = ct.c_int() + return c_GetPingTime(clientID, ct.byref(pingTime)), pingTime.value + +def simxGetLastCmdTime(clientID): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_GetLastCmdTime(clientID) + +def simxSynchronousTrigger(clientID): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_SynchronousTrigger(clientID) + +def simxSynchronous(clientID, enable): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_Synchronous(clientID, enable) + +def simxPauseCommunication(clientID, enable): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_PauseCommunication(clientID, enable) + +def simxGetInMessageInfo(clientID, infoType): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + info = ct.c_int() + return c_GetInMessageInfo(clientID, infoType, ct.byref(info)), info.value + +def simxGetOutMessageInfo(clientID, infoType): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + info = ct.c_int() + return c_GetOutMessageInfo(clientID, infoType, ct.byref(info)), info.value + +def simxGetConnectionId(clientID): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_GetConnectionId(clientID) + +def simxCreateBuffer(bufferSize): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_CreateBuffer(bufferSize) + +def simxReleaseBuffer(buffer): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + return c_ReleaseBuffer(buffer) + +def simxTransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(filePathAndName) is str): + filePathAndName=filePathAndName.encode('utf-8') + return c_TransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode) + +def simxEraseFile(clientID, fileName_serverSide, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if (sys.version_info[0] == 3) and (type(fileName_serverSide) is str): + fileName_serverSide=fileName_serverSide.encode('utf-8') + return c_EraseFile(clientID, fileName_serverSide, operationMode) + +def simxCreateDummy(clientID, size, color, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + handle = ct.c_int() + if color != None: + c_color = (ct.c_ubyte*12)(*color) + else: + c_color = None + return c_CreateDummy(clientID, size, c_color, ct.byref(handle), operationMode), handle.value + +def simxQuery(clientID, signalName, signalValue, retSignalName, timeOutInMs): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + retSignalLength = ct.c_int(); + retSignalValue = ct.POINTER(ct.c_ubyte)() + + sigV=signalValue + if sys.version_info[0] == 3: + if type(signalName) is str: + signalName=signalName.encode('utf-8') + if type(retSignalName) is str: + retSignalName=retSignalName.encode('utf-8') + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=signalValue.encode('utf-8') + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + else: + if type(signalValue) is bytearray: + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + if type(signalValue) is str: + signalValue=bytearray(signalValue) + sigV = (ct.c_ubyte*len(signalValue))(*signalValue) + sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + + ret = c_Query(clientID, signalName, sigV, len(signalValue), retSignalName, ct.byref(retSignalValue), ct.byref(retSignalLength), timeOutInMs) + + a = bytearray() + if ret == 0: + for i in range(retSignalLength.value): + a.append(retSignalValue[i]) + if sys.version_info[0] != 3: + a=str(a) + + return ret, a + +def simxGetObjectGroupData(clientID, objectType, dataType, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + handles =[] + intData =[] + floatData =[] + stringData =[] + handlesC = ct.c_int() + handlesP = ct.POINTER(ct.c_int)() + intDataC = ct.c_int() + intDataP = ct.POINTER(ct.c_int)() + floatDataC = ct.c_int() + floatDataP = ct.POINTER(ct.c_float)() + stringDataC = ct.c_int() + stringDataP = ct.POINTER(ct.c_char)() + ret = c_GetObjectGroupData(clientID, objectType, dataType, ct.byref(handlesC), ct.byref(handlesP), ct.byref(intDataC), ct.byref(intDataP), ct.byref(floatDataC), ct.byref(floatDataP), ct.byref(stringDataC), ct.byref(stringDataP), operationMode) + + if ret == 0: + for i in range(handlesC.value): + handles.append(handlesP[i]) + for i in range(intDataC.value): + intData.append(intDataP[i]) + for i in range(floatDataC.value): + floatData.append(floatDataP[i]) + s = 0 + for i in range(stringDataC.value): + a = bytearray() + while stringDataP[s] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(stringDataP[s],'big')) + else: + a.append(stringDataP[s]) + s += 1 + s += 1 #skip null + if sys.version_info[0] == 3: + a=str(a,'utf-8') + else: + a=str(a) + stringData.append(a) + + return ret, handles, intData, floatData, stringData + +def simxCallScriptFunction(clientID, scriptDescription, options, functionName, inputInts, inputFloats, inputStrings, inputBuffer, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + inputBufferV=inputBuffer + if sys.version_info[0] == 3: + if type(scriptDescription) is str: + scriptDescription=scriptDescription.encode('utf-8') + if type(functionName) is str: + functionName=functionName.encode('utf-8') + if type(inputBuffer) is bytearray: + inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) + if type(inputBuffer) is str: + inputBuffer=inputBuffer.encode('utf-8') + inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) + else: + if type(inputBuffer) is bytearray: + inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) + if type(inputBuffer) is str: + inputBuffer=bytearray(inputBuffer) + inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) + inputBufferV=ct.cast(inputBufferV,ct.POINTER(ct.c_ubyte)) # IronPython needs this + + c_inInts = (ct.c_int*len(inputInts))(*inputInts) + c_inInts = ct.cast(c_inInts,ct.POINTER(ct.c_int)) # IronPython needs this + c_inFloats = (ct.c_float*len(inputFloats))(*inputFloats) + c_inFloats = ct.cast(c_inFloats,ct.POINTER(ct.c_float)) # IronPython needs this + + concatStr=''.encode('utf-8') + for i in range(len(inputStrings)): + a=inputStrings[i] + a=a+'\0' + if type(a) is str: + a=a.encode('utf-8') + concatStr=concatStr+a + c_inStrings = (ct.c_char*len(concatStr))(*concatStr) + + intDataOut =[] + floatDataOut =[] + stringDataOut =[] + bufferOut =bytearray() + + intDataC = ct.c_int() + intDataP = ct.POINTER(ct.c_int)() + floatDataC = ct.c_int() + floatDataP = ct.POINTER(ct.c_float)() + stringDataC = ct.c_int() + stringDataP = ct.POINTER(ct.c_char)() + bufferS = ct.c_int() + bufferP = ct.POINTER(ct.c_ubyte)() + + ret = c_CallScriptFunction(clientID,scriptDescription,options,functionName,len(inputInts),c_inInts,len(inputFloats),c_inFloats,len(inputStrings),c_inStrings,len(inputBuffer),inputBufferV,ct.byref(intDataC),ct.byref(intDataP),ct.byref(floatDataC),ct.byref(floatDataP),ct.byref(stringDataC),ct.byref(stringDataP),ct.byref(bufferS),ct.byref(bufferP),operationMode) + + if ret == 0: + for i in range(intDataC.value): + intDataOut.append(intDataP[i]) + for i in range(floatDataC.value): + floatDataOut.append(floatDataP[i]) + s = 0 + for i in range(stringDataC.value): + a = bytearray() + while stringDataP[s] != b'\0': + if sys.version_info[0] == 3: + a.append(int.from_bytes(stringDataP[s],'big')) + else: + a.append(stringDataP[s]) + s += 1 + s += 1 #skip null + if sys.version_info[0] == 3: + a=str(a,'utf-8') + else: + a=str(a) + stringDataOut.append(a) + for i in range(bufferS.value): + bufferOut.append(bufferP[i]) + if sys.version_info[0] != 3: + bufferOut=str(bufferOut) + + return ret, intDataOut, floatDataOut, stringDataOut, bufferOut + +def simxGetObjectVelocity(clientID, objectHandle, operationMode): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + linearVel = (ct.c_float*3)() + angularVel = (ct.c_float*3)() + ret = c_GetObjectVelocity(clientID, objectHandle, linearVel, angularVel, operationMode) + arr1 = [] + for i in range(3): + arr1.append(linearVel[i]) + arr2 = [] + for i in range(3): + arr2.append(angularVel[i]) + return ret, arr1, arr2 + +def simxPackInts(intList): + ''' + Please have a look at the function description/documentation in the V-REP user manual + ''' + + if sys.version_info[0] == 3: + s=bytes() + for i in range(len(intList)): + s=s+struct.pack('. +# ------------------------------------------------------------------- +# +# This file was automatically created for V-REP release V3.3.2 on August 29th 2016 + +#constants +#Scene object types. Values are serialized +sim_object_shape_type =0 +sim_object_joint_type =1 +sim_object_graph_type =2 +sim_object_camera_type =3 +sim_object_dummy_type =4 +sim_object_proximitysensor_type =5 +sim_object_reserved1 =6 +sim_object_reserved2 =7 +sim_object_path_type =8 +sim_object_visionsensor_type =9 +sim_object_volume_type =10 +sim_object_mill_type =11 +sim_object_forcesensor_type =12 +sim_object_light_type =13 +sim_object_mirror_type =14 + +#General object types. Values are serialized +sim_appobj_object_type =109 +sim_appobj_collision_type =110 +sim_appobj_distance_type =111 +sim_appobj_simulation_type =112 +sim_appobj_ik_type =113 +sim_appobj_constraintsolver_type=114 +sim_appobj_collection_type =115 +sim_appobj_ui_type =116 +sim_appobj_script_type =117 +sim_appobj_pathplanning_type =118 +sim_appobj_RESERVED_type =119 +sim_appobj_texture_type =120 + +# Ik calculation methods. Values are serialized +sim_ik_pseudo_inverse_method =0 +sim_ik_damped_least_squares_method =1 +sim_ik_jacobian_transpose_method =2 + +# Ik constraints. Values are serialized +sim_ik_x_constraint =1 +sim_ik_y_constraint =2 +sim_ik_z_constraint =4 +sim_ik_alpha_beta_constraint=8 +sim_ik_gamma_constraint =16 +sim_ik_avoidance_constraint =64 + +# Ik calculation results +sim_ikresult_not_performed =0 +sim_ikresult_success =1 +sim_ikresult_fail =2 + +# Scene object sub-types. Values are serialized +# Light sub-types +sim_light_omnidirectional_subtype =1 +sim_light_spot_subtype =2 +sim_light_directional_subtype =3 +# Joint sub-types +sim_joint_revolute_subtype =10 +sim_joint_prismatic_subtype =11 +sim_joint_spherical_subtype =12 +# Shape sub-types +sim_shape_simpleshape_subtype =20 +sim_shape_multishape_subtype =21 +# Proximity sensor sub-types +sim_proximitysensor_pyramid_subtype =30 +sim_proximitysensor_cylinder_subtype=31 +sim_proximitysensor_disc_subtype =32 +sim_proximitysensor_cone_subtype =33 +sim_proximitysensor_ray_subtype =34 +# Mill sub-types +sim_mill_pyramid_subtype =40 +sim_mill_cylinder_subtype =41 +sim_mill_disc_subtype =42 +sim_mill_cone_subtype =42 +# No sub-type +sim_object_no_subtype =200 + + +#Scene object main properties (serialized) +sim_objectspecialproperty_collidable =0x0001 +sim_objectspecialproperty_measurable =0x0002 +#reserved =0x0004 +#reserved =0x0008 +sim_objectspecialproperty_detectable_ultrasonic =0x0010 +sim_objectspecialproperty_detectable_infrared =0x0020 +sim_objectspecialproperty_detectable_laser =0x0040 +sim_objectspecialproperty_detectable_inductive =0x0080 +sim_objectspecialproperty_detectable_capacitive =0x0100 +sim_objectspecialproperty_renderable =0x0200 +sim_objectspecialproperty_detectable_all =sim_objectspecialproperty_detectable_ultrasonic|sim_objectspecialproperty_detectable_infrared|sim_objectspecialproperty_detectable_laser|sim_objectspecialproperty_detectable_inductive|sim_objectspecialproperty_detectable_capacitive +sim_objectspecialproperty_cuttable =0x0400 +sim_objectspecialproperty_pathplanning_ignored =0x0800 + +# Model properties (serialized) +sim_modelproperty_not_collidable =0x0001 +sim_modelproperty_not_measurable =0x0002 +sim_modelproperty_not_renderable =0x0004 +sim_modelproperty_not_detectable =0x0008 +sim_modelproperty_not_cuttable =0x0010 +sim_modelproperty_not_dynamic =0x0020 +sim_modelproperty_not_respondable =0x0040 # cannot be selected if sim_modelproperty_not_dynamic is not selected +sim_modelproperty_not_reset =0x0080 # Model is not reset at simulation end. This flag is cleared at simulation end +sim_modelproperty_not_visible =0x0100 # Whole model is invisible independent of local visibility settings +sim_modelproperty_not_model =0xf000 # object is not a model + + +# Check the documentation instead of comments below!! +# Following messages are dispatched to the Lua-message container +sim_message_ui_button_state_change =0 # a UI button slider etc. changed (due to a user's action). aux[0]=UI handle aux[1]=button handle aux[2]=button attributes aux[3]=slider position (if slider) +sim_message_reserved9 =1 # Do not use +sim_message_object_selection_changed=2 +sim_message_reserved10 =3 # do not use +sim_message_model_loaded =4 +sim_message_reserved11 =5 # do not use +sim_message_keypress =6 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state) +sim_message_bannerclicked =7 # a banner was clicked (aux[0]=banner ID) + + +# Following messages are dispatched only to the C-API (not available from Lua) +sim_message_for_c_api_only_start =0x100 # Do not use +sim_message_reserved1 =0x101 # Do not use +sim_message_reserved2 =0x102 # Do not use +sim_message_reserved3 =0x103 # Do not use +sim_message_eventcallback_scenesave =0x104 # about to save a scene +sim_message_eventcallback_modelsave =0x105 # about to save a model (current selection will be saved) +sim_message_eventcallback_moduleopen =0x106 # called when simOpenModule in Lua is called +sim_message_eventcallback_modulehandle =0x107 # called when simHandleModule in Lua is called with argument false +sim_message_eventcallback_moduleclose =0x108 # called when simCloseModule in Lua is called +sim_message_reserved4 =0x109 # Do not use +sim_message_reserved5 =0x10a # Do not use +sim_message_reserved6 =0x10b # Do not use +sim_message_reserved7 =0x10c # Do not use +sim_message_eventcallback_instancepass =0x10d # Called once every main application loop pass. auxiliaryData[0] contains event flags of events that happened since last time +sim_message_eventcallback_broadcast =0x10e +sim_message_eventcallback_imagefilter_enumreset =0x10f +sim_message_eventcallback_imagefilter_enumerate =0x110 +sim_message_eventcallback_imagefilter_adjustparams =0x111 +sim_message_eventcallback_imagefilter_reserved =0x112 +sim_message_eventcallback_imagefilter_process =0x113 +sim_message_eventcallback_reserved1 =0x114 # do not use +sim_message_eventcallback_reserved2 =0x115 # do not use +sim_message_eventcallback_reserved3 =0x116 # do not use +sim_message_eventcallback_reserved4 =0x117 # do not use +sim_message_eventcallback_abouttoundo =0x118 # the undo button was hit and a previous state is about to be restored +sim_message_eventcallback_undoperformed =0x119 # the undo button was hit and a previous state restored +sim_message_eventcallback_abouttoredo =0x11a # the redo button was hit and a future state is about to be restored +sim_message_eventcallback_redoperformed =0x11b # the redo button was hit and a future state restored +sim_message_eventcallback_scripticondblclick =0x11c # scipt icon was double clicked. (aux[0]=object handle associated with script set replyData[0] to 1 if script should not be opened) +sim_message_eventcallback_simulationabouttostart =0x11d +sim_message_eventcallback_simulationended =0x11e +sim_message_eventcallback_reserved5 =0x11f # do not use +sim_message_eventcallback_keypress =0x120 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state) +sim_message_eventcallback_modulehandleinsensingpart =0x121 # called when simHandleModule in Lua is called with argument true +sim_message_eventcallback_renderingpass =0x122 # called just before the scene is rendered +sim_message_eventcallback_bannerclicked =0x123 # called when a banner was clicked (aux[0]=banner ID) +sim_message_eventcallback_menuitemselected =0x124 # auxiliaryData[0] indicates the handle of the item auxiliaryData[1] indicates the state of the item +sim_message_eventcallback_refreshdialogs =0x125 # aux[0]=refresh degree (0=light 1=medium 2=full) +sim_message_eventcallback_sceneloaded =0x126 +sim_message_eventcallback_modelloaded =0x127 +sim_message_eventcallback_instanceswitch =0x128 +sim_message_eventcallback_guipass =0x129 +sim_message_eventcallback_mainscriptabouttobecalled =0x12a +sim_message_eventcallback_rmlposition =0x12b #the command simRMLPosition was called. The appropriate plugin should handle the call +sim_message_eventcallback_rmlvelocity =0x12c # the command simRMLVelocity was called. The appropriate plugin should handle the call +sim_message_simulation_start_resume_request =0x1000 +sim_message_simulation_pause_request =0x1001 +sim_message_simulation_stop_request =0x1002 + +# Scene object properties. Combine with the | operator +sim_objectproperty_reserved1 =0x0000 +sim_objectproperty_reserved2 =0x0001 +sim_objectproperty_reserved3 =0x0002 +sim_objectproperty_reserved4 =0x0003 +sim_objectproperty_reserved5 =0x0004 # formely sim_objectproperty_visible +sim_objectproperty_reserved6 =0x0008 # formely sim_objectproperty_wireframe +sim_objectproperty_collapsed =0x0010 +sim_objectproperty_selectable =0x0020 +sim_objectproperty_reserved7 =0x0040 +sim_objectproperty_selectmodelbaseinstead =0x0080 +sim_objectproperty_dontshowasinsidemodel =0x0100 +# reserved =0x0200 +sim_objectproperty_canupdatedna =0x0400 +sim_objectproperty_selectinvisible =0x0800 +sim_objectproperty_depthinvisible =0x1000 + + +# type of arguments (input and output) for custom lua commands +sim_lua_arg_nil =0 +sim_lua_arg_bool =1 +sim_lua_arg_int =2 +sim_lua_arg_float =3 +sim_lua_arg_string =4 +sim_lua_arg_invalid =5 +sim_lua_arg_table =8 + +# custom user interface properties. Values are serialized. +sim_ui_property_visible =0x0001 +sim_ui_property_visibleduringsimulationonly =0x0002 +sim_ui_property_moveable =0x0004 +sim_ui_property_relativetoleftborder =0x0008 +sim_ui_property_relativetotopborder =0x0010 +sim_ui_property_fixedwidthfont =0x0020 +sim_ui_property_systemblock =0x0040 +sim_ui_property_settocenter =0x0080 +sim_ui_property_rolledup =0x0100 +sim_ui_property_selectassociatedobject =0x0200 +sim_ui_property_visiblewhenobjectselected =0x0400 + + +# button properties. Values are serialized. +sim_buttonproperty_button =0x0000 +sim_buttonproperty_label =0x0001 +sim_buttonproperty_slider =0x0002 +sim_buttonproperty_editbox =0x0003 +sim_buttonproperty_staydown =0x0008 +sim_buttonproperty_enabled =0x0010 +sim_buttonproperty_borderless =0x0020 +sim_buttonproperty_horizontallycentered =0x0040 +sim_buttonproperty_ignoremouse =0x0080 +sim_buttonproperty_isdown =0x0100 +sim_buttonproperty_transparent =0x0200 +sim_buttonproperty_nobackgroundcolor =0x0400 +sim_buttonproperty_rollupaction =0x0800 +sim_buttonproperty_closeaction =0x1000 +sim_buttonproperty_verticallycentered =0x2000 +sim_buttonproperty_downupevent =0x4000 + + +# Simulation status +sim_simulation_stopped =0x00 # Simulation is stopped +sim_simulation_paused =0x08 # Simulation is paused +sim_simulation_advancing =0x10 # Simulation is advancing +sim_simulation_advancing_firstafterstop =sim_simulation_advancing|0x00 # First simulation pass (1x) +sim_simulation_advancing_running =sim_simulation_advancing|0x01 # Normal simulation pass (>=1x) +# reserved =sim_simulation_advancing|0x02 +sim_simulation_advancing_lastbeforepause =sim_simulation_advancing|0x03 # Last simulation pass before pause (1x) +sim_simulation_advancing_firstafterpause =sim_simulation_advancing|0x04 # First simulation pass after pause (1x) +sim_simulation_advancing_abouttostop =sim_simulation_advancing|0x05 # "Trying to stop" simulation pass (>=1x) +sim_simulation_advancing_lastbeforestop =sim_simulation_advancing|0x06 # Last simulation pass (1x) + + +# Script execution result (first return value) +sim_script_no_error =0 +sim_script_main_script_nonexistent =1 +sim_script_main_script_not_called =2 +sim_script_reentrance_error =4 +sim_script_lua_error =8 +sim_script_call_error =16 + + + # Script types (serialized!) +sim_scripttype_mainscript =0 +sim_scripttype_childscript =1 +sim_scripttype_jointctrlcallback =4 +sim_scripttype_contactcallback =5 +sim_scripttype_customizationscript =6 +sim_scripttype_generalcallback =7 + +# API call error messages +sim_api_errormessage_ignore =0 # does not memorize nor output errors +sim_api_errormessage_report =1 # memorizes errors (default for C-API calls) +sim_api_errormessage_output =2 # memorizes and outputs errors (default for Lua-API calls) + + +# special argument of some functions +sim_handle_all =-2 +sim_handle_all_except_explicit =-3 +sim_handle_self =-4 +sim_handle_main_script =-5 +sim_handle_tree =-6 +sim_handle_chain =-7 +sim_handle_single =-8 +sim_handle_default =-9 +sim_handle_all_except_self =-10 +sim_handle_parent =-11 + + +# special handle flags +sim_handleflag_assembly =0x400000 +sim_handleflag_model =0x800000 + + +# distance calculation methods (serialized) +sim_distcalcmethod_dl =0 +sim_distcalcmethod_dac =1 +sim_distcalcmethod_max_dl_dac =2 +sim_distcalcmethod_dl_and_dac =3 +sim_distcalcmethod_sqrt_dl2_and_dac2=4 +sim_distcalcmethod_dl_if_nonzero =5 +sim_distcalcmethod_dac_if_nonzero =6 + + + # Generic dialog styles +sim_dlgstyle_message =0 +sim_dlgstyle_input =1 +sim_dlgstyle_ok =2 +sim_dlgstyle_ok_cancel =3 +sim_dlgstyle_yes_no =4 +sim_dlgstyle_dont_center =32# can be combined with one of above values. Only with this flag can the position of the related UI be set just after dialog creation + + # Generic dialog return values +sim_dlgret_still_open =0 +sim_dlgret_ok =1 +sim_dlgret_cancel =2 +sim_dlgret_yes =3 +sim_dlgret_no =4 + + +# Path properties +sim_pathproperty_show_line =0x0001 +sim_pathproperty_show_orientation =0x0002 +sim_pathproperty_closed_path =0x0004 +sim_pathproperty_automatic_orientation =0x0008 +sim_pathproperty_invert_velocity =0x0010 +sim_pathproperty_infinite_acceleration =0x0020 +sim_pathproperty_flat_path =0x0040 +sim_pathproperty_show_position =0x0080 +sim_pathproperty_auto_velocity_profile_translation =0x0100 +sim_pathproperty_auto_velocity_profile_rotation =0x0200 +sim_pathproperty_endpoints_at_zero =0x0400 +sim_pathproperty_keep_x_up =0x0800 + + + # drawing objects +# following are mutually exclusive +sim_drawing_points =0 # 3 values per point (point size in pixels) +sim_drawing_lines =1 # 6 values per line (line size in pixels) +sim_drawing_triangles =2 # 9 values per triangle +sim_drawing_trianglepoints =3 # 6 values per point (3 for triangle position 3 for triangle normal vector) (triangle size in meters) +sim_drawing_quadpoints =4 # 6 values per point (3 for quad position 3 for quad normal vector) (quad size in meters) +sim_drawing_discpoints =5 # 6 values per point (3 for disc position 3 for disc normal vector) (disc size in meters) +sim_drawing_cubepoints =6 # 6 values per point (3 for cube position 3 for cube normal vector) (cube size in meters) +sim_drawing_spherepoints =7 # 3 values per point (sphere size in meters) + +# following can be or-combined +sim_drawing_itemcolors =0x00020 # +3 values per item (each item has its own ambient color (rgb values)). + # Mutually exclusive with sim_drawing_vertexcolors +sim_drawing_vertexcolors =0x00040 # +3 values per vertex (each vertex has its own ambient color (rgb values). Only for sim_drawing_lines (+6) and for sim_drawing_triangles(+9)). Mutually exclusive with sim_drawing_itemcolors +sim_drawing_itemsizes =0x00080 # +1 value per item (each item has its own size). Not for sim_drawing_triangles +sim_drawing_backfaceculling =0x00100 # back faces are not displayed for all items +sim_drawing_wireframe =0x00200 # all items displayed in wireframe +sim_drawing_painttag =0x00400 # all items are tagged as paint (for additinal processing at a later stage) +sim_drawing_followparentvisibility =0x00800 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible +sim_drawing_cyclic =0x01000 # if the max item count was reached then the first items are overwritten. +sim_drawing_50percenttransparency =0x02000 # the drawing object will be 50% transparent +sim_drawing_25percenttransparency =0x04000 # the drawing object will be 25% transparent +sim_drawing_12percenttransparency =0x08000 # the drawing object will be 12.5% transparent +sim_drawing_emissioncolor =0x10000 # When used in combination with sim_drawing_itemcolors or sim_drawing_vertexcolors then the specified colors will be for the emissive component +sim_drawing_facingcamera =0x20000 # Only for trianglepoints quadpoints discpoints and cubepoints. If specified the normal verctor is calculated to face the camera (each item data requires 3 values less) +sim_drawing_overlay =0x40000 # When specified objects are always drawn on top of "regular objects" +sim_drawing_itemtransparency =0x80000 # +1 value per item (each item has its own transparency value (0-1)). Not compatible with sim_drawing_vertexcolors + +# banner values +# following can be or-combined +sim_banner_left =0x00001 # Banners display on the left of the specified point +sim_banner_right =0x00002 # Banners display on the right of the specified point +sim_banner_nobackground =0x00004 # Banners have no background rectangle +sim_banner_overlay =0x00008 # When specified banners are always drawn on top of "regular objects" +sim_banner_followparentvisibility =0x00010 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible +sim_banner_clickselectsparent =0x00020 # if the object is associated with a scene object then clicking the banner will select the scene object +sim_banner_clicktriggersevent =0x00040 # if the banner is clicked an event is triggered (sim_message_eventcallback_bannerclicked and sim_message_bannerclicked are generated) +sim_banner_facingcamera =0x00080 # If specified the banner will always face the camera by rotating around the banner's vertical axis (y-axis) +sim_banner_fullyfacingcamera =0x00100 # If specified the banner will always fully face the camera (the banner's orientation is same as the camera looking at it) +sim_banner_backfaceculling =0x00200 # If specified the banner will only be visible from one side +sim_banner_keepsamesize =0x00400 # If specified the banner will always appear in the same size. In that case size represents the character height in pixels +sim_banner_bitmapfont =0x00800 # If specified a fixed-size bitmap font is used. The text will also always fully face the camera and be right + # to the specified position. Bitmap fonts are not clickable + + +# particle objects following are mutually exclusive +sim_particle_points1 =0 # 6 values per point (pt1 and pt2. Pt1 is start position pt2-pt1 is the initial velocity vector). i + #Point is 1 pixel big. Only appearance is a point internally handled as a perfect sphere +sim_particle_points2 =1 # 6 values per point. Point is 2 pixel big. Only appearance is a point internally handled as a perfect sphere +sim_particle_points4 =2 # 6 values per point. Point is 4 pixel big. Only appearance is a point internally handled as a perfect sphere +sim_particle_roughspheres =3 # 6 values per sphere. Only appearance is rough. Internally a perfect sphere +sim_particle_spheres =4 # 6 values per sphere. Internally a perfect sphere + + + + +# following can be or-combined +sim_particle_respondable1to4 =0x0020 # the particles are respondable against shapes (against all objects that have at least one bit 1-4 activated in the global respondable mask) +sim_particle_respondable5to8 =0x0040 # the particles are respondable against shapes (against all objects that have at least one bit 5-8 activated in the global respondable mask) +sim_particle_particlerespondable =0x0080 # the particles are respondable against each other +sim_particle_ignoresgravity =0x0100 # the particles ignore the effect of gravity. Not compatible with sim_particle_water +sim_particle_invisible =0x0200 # the particles are invisible +sim_particle_itemsizes =0x0400 # +1 value per particle (each particle can have a different size) +sim_particle_itemdensities =0x0800 # +1 value per particle (each particle can have a different density) +sim_particle_itemcolors =0x1000 # +3 values per particle (each particle can have a different color) +sim_particle_cyclic =0x2000 # if the max item count was reached then the first items are overwritten. +sim_particle_emissioncolor =0x4000 # When used in combination with sim_particle_itemcolors then the specified colors will be for the emissive component +sim_particle_water =0x8000 # the particles are water particles (no weight in the water (i.e. when z<0)). Not compatible with sim_particle_ignoresgravity +sim_particle_painttag =0x10000 # The particles can be seen by vision sensors (sim_particle_invisible must not be set) + + + + +# custom user interface menu attributes +sim_ui_menu_title =1 +sim_ui_menu_minimize =2 +sim_ui_menu_close =4 +sim_ui_menu_systemblock =8 + + + +# Boolean parameters +sim_boolparam_hierarchy_visible =0 +sim_boolparam_console_visible =1 +sim_boolparam_collision_handling_enabled =2 +sim_boolparam_distance_handling_enabled =3 +sim_boolparam_ik_handling_enabled =4 +sim_boolparam_gcs_handling_enabled =5 +sim_boolparam_dynamics_handling_enabled =6 +sim_boolparam_joint_motion_handling_enabled =7 +sim_boolparam_path_motion_handling_enabled =8 +sim_boolparam_proximity_sensor_handling_enabled =9 +sim_boolparam_vision_sensor_handling_enabled =10 +sim_boolparam_mill_handling_enabled =11 +sim_boolparam_browser_visible =12 +sim_boolparam_scene_and_model_load_messages =13 +sim_reserved0 =14 +sim_boolparam_shape_textures_are_visible =15 +sim_boolparam_display_enabled =16 +sim_boolparam_infotext_visible =17 +sim_boolparam_statustext_open =18 +sim_boolparam_fog_enabled =19 +sim_boolparam_rml2_available =20 +sim_boolparam_rml4_available =21 +sim_boolparam_mirrors_enabled =22 +sim_boolparam_aux_clip_planes_enabled =23 +sim_boolparam_full_model_copy_from_api =24 +sim_boolparam_realtime_simulation =25 +sim_boolparam_force_show_wireless_emission =27 +sim_boolparam_force_show_wireless_reception =28 +sim_boolparam_video_recording_triggered =29 +sim_boolparam_threaded_rendering_enabled =32 +sim_boolparam_fullscreen =33 +sim_boolparam_headless =34 +sim_boolparam_hierarchy_toolbarbutton_enabled =35 +sim_boolparam_browser_toolbarbutton_enabled =36 +sim_boolparam_objectshift_toolbarbutton_enabled =37 +sim_boolparam_objectrotate_toolbarbutton_enabled=38 +sim_boolparam_force_calcstruct_all_visible =39 +sim_boolparam_force_calcstruct_all =40 +sim_boolparam_exit_request =41 +sim_boolparam_play_toolbarbutton_enabled =42 +sim_boolparam_pause_toolbarbutton_enabled =43 +sim_boolparam_stop_toolbarbutton_enabled =44 +sim_boolparam_waiting_for_trigger =45 + + +# Integer parameters +sim_intparam_error_report_mode =0 # Check sim_api_errormessage_... constants above for valid values +sim_intparam_program_version =1 # e.g Version 2.1.4 --> 20104. Can only be read +sim_intparam_instance_count =2 # do not use anymore (always returns 1 since V-REP 2.5.11) +sim_intparam_custom_cmd_start_id =3 # can only be read +sim_intparam_compilation_version =4 # 0=evaluation version 1=full version 2=player version. Can only be read +sim_intparam_current_page =5 +sim_intparam_flymode_camera_handle =6 # can only be read +sim_intparam_dynamic_step_divider =7 # can only be read +sim_intparam_dynamic_engine =8 # 0=Bullet 1=ODE. 2=Vortex. +sim_intparam_server_port_start =9 # can only be read +sim_intparam_server_port_range =10 # can only be read +sim_intparam_visible_layers =11 +sim_intparam_infotext_style =12 +sim_intparam_settings =13 +sim_intparam_edit_mode_type =14 # can only be read +sim_intparam_server_port_next =15 # is initialized at sim_intparam_server_port_start +sim_intparam_qt_version =16 # version of the used Qt framework +sim_intparam_event_flags_read =17 # can only be read +sim_intparam_event_flags_read_clear =18 # can only be read +sim_intparam_platform =19 # can only be read +sim_intparam_scene_unique_id =20 # can only be read +sim_intparam_work_thread_count =21 +sim_intparam_mouse_x =22 +sim_intparam_mouse_y =23 +sim_intparam_core_count =24 +sim_intparam_work_thread_calc_time_ms =25 +sim_intparam_idle_fps =26 +sim_intparam_prox_sensor_select_down =27 +sim_intparam_prox_sensor_select_up =28 +sim_intparam_stop_request_counter =29 +sim_intparam_program_revision =30 +sim_intparam_mouse_buttons =31 +sim_intparam_dynamic_warning_disabled_mask =32 +sim_intparam_simulation_warning_disabled_mask =33 +sim_intparam_scene_index =34 +sim_intparam_motionplanning_seed =35 +sim_intparam_speedmodifier =36 + +# Float parameters +sim_floatparam_rand=0 # random value (0.0-1.0) +sim_floatparam_simulation_time_step =1 +sim_floatparam_stereo_distance =2 + +# String parameters +sim_stringparam_application_path=0 # path of V-REP's executable +sim_stringparam_video_filename=1 +sim_stringparam_app_arg1 =2 +sim_stringparam_app_arg2 =3 +sim_stringparam_app_arg3 =4 +sim_stringparam_app_arg4 =5 +sim_stringparam_app_arg5 =6 +sim_stringparam_app_arg6 =7 +sim_stringparam_app_arg7 =8 +sim_stringparam_app_arg8 =9 +sim_stringparam_app_arg9 =10 +sim_stringparam_scene_path_and_name =13 + +# Array parameters +sim_arrayparam_gravity =0 +sim_arrayparam_fog =1 +sim_arrayparam_fog_color =2 +sim_arrayparam_background_color1=3 +sim_arrayparam_background_color2=4 +sim_arrayparam_ambient_light =5 +sim_arrayparam_random_euler =6 + +sim_objintparam_visibility_layer= 10 +sim_objfloatparam_abs_x_velocity= 11 +sim_objfloatparam_abs_y_velocity= 12 +sim_objfloatparam_abs_z_velocity= 13 +sim_objfloatparam_abs_rot_velocity= 14 +sim_objfloatparam_objbbox_min_x= 15 +sim_objfloatparam_objbbox_min_y= 16 +sim_objfloatparam_objbbox_min_z= 17 +sim_objfloatparam_objbbox_max_x= 18 +sim_objfloatparam_objbbox_max_y= 19 +sim_objfloatparam_objbbox_max_z= 20 +sim_objfloatparam_modelbbox_min_x= 21 +sim_objfloatparam_modelbbox_min_y= 22 +sim_objfloatparam_modelbbox_min_z= 23 +sim_objfloatparam_modelbbox_max_x= 24 +sim_objfloatparam_modelbbox_max_y= 25 +sim_objfloatparam_modelbbox_max_z= 26 +sim_objintparam_collection_self_collision_indicator= 27 +sim_objfloatparam_transparency_offset= 28 +sim_objintparam_child_role= 29 +sim_objintparam_parent_role= 30 +sim_objintparam_manipulation_permissions= 31 +sim_objintparam_illumination_handle= 32 + +sim_visionfloatparam_near_clipping= 1000 +sim_visionfloatparam_far_clipping= 1001 +sim_visionintparam_resolution_x= 1002 +sim_visionintparam_resolution_y= 1003 +sim_visionfloatparam_perspective_angle= 1004 +sim_visionfloatparam_ortho_size= 1005 +sim_visionintparam_disabled_light_components= 1006 +sim_visionintparam_rendering_attributes= 1007 +sim_visionintparam_entity_to_render= 1008 +sim_visionintparam_windowed_size_x= 1009 +sim_visionintparam_windowed_size_y= 1010 +sim_visionintparam_windowed_pos_x= 1011 +sim_visionintparam_windowed_pos_y= 1012 +sim_visionintparam_pov_focal_blur= 1013 +sim_visionfloatparam_pov_blur_distance= 1014 +sim_visionfloatparam_pov_aperture= 1015 +sim_visionintparam_pov_blur_sampled= 1016 +sim_visionintparam_render_mode= 1017 + +sim_jointintparam_motor_enabled= 2000 +sim_jointintparam_ctrl_enabled= 2001 +sim_jointfloatparam_pid_p= 2002 +sim_jointfloatparam_pid_i= 2003 +sim_jointfloatparam_pid_d= 2004 +sim_jointfloatparam_intrinsic_x= 2005 +sim_jointfloatparam_intrinsic_y= 2006 +sim_jointfloatparam_intrinsic_z= 2007 +sim_jointfloatparam_intrinsic_qx= 2008 +sim_jointfloatparam_intrinsic_qy= 2009 +sim_jointfloatparam_intrinsic_qz= 2010 +sim_jointfloatparam_intrinsic_qw= 2011 +sim_jointfloatparam_velocity= 2012 +sim_jointfloatparam_spherical_qx= 2013 +sim_jointfloatparam_spherical_qy= 2014 +sim_jointfloatparam_spherical_qz= 2015 +sim_jointfloatparam_spherical_qw= 2016 +sim_jointfloatparam_upper_limit= 2017 +sim_jointfloatparam_kc_k= 2018 +sim_jointfloatparam_kc_c= 2019 +sim_jointfloatparam_ik_weight= 2021 +sim_jointfloatparam_error_x= 2022 +sim_jointfloatparam_error_y= 2023 +sim_jointfloatparam_error_z= 2024 +sim_jointfloatparam_error_a= 2025 +sim_jointfloatparam_error_b= 2026 +sim_jointfloatparam_error_g= 2027 +sim_jointfloatparam_error_pos= 2028 +sim_jointfloatparam_error_angle= 2029 +sim_jointintparam_velocity_lock= 2030 +sim_jointintparam_vortex_dep_handle= 2031 +sim_jointfloatparam_vortex_dep_multiplication= 2032 +sim_jointfloatparam_vortex_dep_offset= 2033 + +sim_shapefloatparam_init_velocity_x= 3000 +sim_shapefloatparam_init_velocity_y= 3001 +sim_shapefloatparam_init_velocity_z= 3002 +sim_shapeintparam_static= 3003 +sim_shapeintparam_respondable= 3004 +sim_shapefloatparam_mass= 3005 +sim_shapefloatparam_texture_x= 3006 +sim_shapefloatparam_texture_y= 3007 +sim_shapefloatparam_texture_z= 3008 +sim_shapefloatparam_texture_a= 3009 +sim_shapefloatparam_texture_b= 3010 +sim_shapefloatparam_texture_g= 3011 +sim_shapefloatparam_texture_scaling_x= 3012 +sim_shapefloatparam_texture_scaling_y= 3013 +sim_shapeintparam_culling= 3014 +sim_shapeintparam_wireframe= 3015 +sim_shapeintparam_compound= 3016 +sim_shapeintparam_convex= 3017 +sim_shapeintparam_convex_check= 3018 +sim_shapeintparam_respondable_mask= 3019 +sim_shapefloatparam_init_velocity_a= 3020 +sim_shapefloatparam_init_velocity_b= 3021 +sim_shapefloatparam_init_velocity_g= 3022 +sim_shapestringparam_color_name= 3023 +sim_shapeintparam_edge_visibility= 3024 +sim_shapefloatparam_shading_angle= 3025 +sim_shapefloatparam_edge_angle= 3026 +sim_shapeintparam_edge_borders_hidden= 3027 + +sim_proxintparam_ray_invisibility= 4000 + +sim_forcefloatparam_error_x= 5000 +sim_forcefloatparam_error_y= 5001 +sim_forcefloatparam_error_z= 5002 +sim_forcefloatparam_error_a= 5003 +sim_forcefloatparam_error_b= 5004 +sim_forcefloatparam_error_g= 5005 +sim_forcefloatparam_error_pos= 5006 +sim_forcefloatparam_error_angle= 5007 + +sim_lightintparam_pov_casts_shadows= 8000 + +sim_cameraintparam_disabled_light_components= 9000 +sim_camerafloatparam_perspective_angle= 9001 +sim_camerafloatparam_ortho_size= 9002 +sim_cameraintparam_rendering_attributes= 9003 +sim_cameraintparam_pov_focal_blur= 9004 +sim_camerafloatparam_pov_blur_distance= 9005 +sim_camerafloatparam_pov_aperture= 9006 +sim_cameraintparam_pov_blur_samples= 9007 + +sim_dummyintparam_link_type= 10000 + +sim_mirrorfloatparam_width= 12000 +sim_mirrorfloatparam_height= 12001 +sim_mirrorfloatparam_reflectance= 12002 +sim_mirrorintparam_enable= 12003 + +sim_pplanfloatparam_x_min= 20000 +sim_pplanfloatparam_x_range= 20001 +sim_pplanfloatparam_y_min= 20002 +sim_pplanfloatparam_y_range= 20003 +sim_pplanfloatparam_z_min= 20004 +sim_pplanfloatparam_z_range= 20005 +sim_pplanfloatparam_delta_min= 20006 +sim_pplanfloatparam_delta_range= 20007 + +sim_mplanintparam_nodes_computed= 25000 +sim_mplanintparam_prepare_nodes= 25001 +sim_mplanintparam_clear_nodes= 25002 + +# User interface elements +sim_gui_menubar =0x0001 +sim_gui_popups =0x0002 +sim_gui_toolbar1 =0x0004 +sim_gui_toolbar2 =0x0008 +sim_gui_hierarchy =0x0010 +sim_gui_infobar =0x0020 +sim_gui_statusbar =0x0040 +sim_gui_scripteditor =0x0080 +sim_gui_scriptsimulationparameters =0x0100 +sim_gui_dialogs =0x0200 +sim_gui_browser =0x0400 +sim_gui_all =0xffff + + +# Joint modes +sim_jointmode_passive =0 +sim_jointmode_motion =1 +sim_jointmode_ik =2 +sim_jointmode_ikdependent =3 +sim_jointmode_dependent =4 +sim_jointmode_force =5 + + +# Navigation and selection modes with the mouse. Lower byte values are mutually exclusive upper byte bits can be combined +sim_navigation_passive =0x0000 +sim_navigation_camerashift =0x0001 +sim_navigation_camerarotate =0x0002 +sim_navigation_camerazoom =0x0003 +sim_navigation_cameratilt =0x0004 +sim_navigation_cameraangle =0x0005 +sim_navigation_camerafly =0x0006 +sim_navigation_objectshift =0x0007 +sim_navigation_objectrotate =0x0008 +sim_navigation_reserved2 =0x0009 +sim_navigation_reserved3 =0x000A +sim_navigation_jointpathtest =0x000B +sim_navigation_ikmanip =0x000C +sim_navigation_objectmultipleselection =0x000D +# Bit-combine following values and add them to one of above's values for a valid navigation mode +sim_navigation_reserved4 =0x0100 +sim_navigation_clickselection =0x0200 +sim_navigation_ctrlselection =0x0400 +sim_navigation_shiftselection =0x0800 +sim_navigation_camerazoomwheel =0x1000 +sim_navigation_camerarotaterightbutton =0x2000 + + + +#Remote API constants +SIMX_VERSION =0 +# Remote API message header structure +SIMX_HEADER_SIZE =18 +simx_headeroffset_crc =0 # 1 simxUShort. Generated by the client or server. The CRC for the message +simx_headeroffset_version =2 # 1 byte. Generated by the client or server. The version of the remote API software +simx_headeroffset_message_id =3 # 1 simxInt. Generated by the client (and used in a reply by the server) +simx_headeroffset_client_time =7 # 1 simxInt. Client time stamp generated by the client (and sent back by the server) +simx_headeroffset_server_time =11 # 1 simxInt. Generated by the server when a reply is generated. The server timestamp +simx_headeroffset_scene_id =15 # 1 simxUShort. Generated by the server. A unique ID identifying the scene currently displayed +simx_headeroffset_server_state =17 # 1 byte. Generated by the server. Bit coded 0 set --> simulation not stopped 1 set --> simulation paused 2 set --> real-time switch on 3-5 edit mode type (0=no edit mode 1=triangle 2=vertex 3=edge 4=path 5=UI) + +# Remote API command header +SIMX_SUBHEADER_SIZE =26 +simx_cmdheaderoffset_mem_size =0 # 1 simxInt. Generated by the client or server. The buffer size of the command. +simx_cmdheaderoffset_full_mem_size =4 # 1 simxInt. Generated by the client or server. The full buffer size of the command (applies to split chunks). +simx_cmdheaderoffset_pdata_offset0 =8 # 1 simxUShort. Generated by the client or server. The amount of data that is part of the command identification. +simx_cmdheaderoffset_pdata_offset1 =10 # 1 simxInt. Generated by the client or server. The amount of shift of the pure data buffer (applies to split chunks). +simx_cmdheaderoffset_cmd=14 # 1 simxInt. Generated by the client (and used in a reply by the server). The command combined with the operation mode of the command. +simx_cmdheaderoffset_delay_or_split =18 # 1 simxUShort. Generated by the client or server. The amount of delay in ms of a continuous command or the max. pure data size to send at once (applies to split commands). +simx_cmdheaderoffset_sim_time =20 # 1 simxInt. Generated by the server. The simulation time (in ms) when the command was executed (or 0 if simulation is not running) +simx_cmdheaderoffset_status =24 # 1 byte. Generated by the server. (1 bit 0 is set --> error in function execution on server side). The client writes bit 1 if command cannot be overwritten +simx_cmdheaderoffset_reserved =25 # 1 byte. Not yet used + + + + + +# Regular operation modes +simx_opmode_oneshot =0x000000 # sends command as one chunk. Reply will also come as one chunk. Doesn't wait for the reply. +simx_opmode_blocking =0x010000 # sends command as one chunk. Reply will also come as one chunk. Waits for the reply (_REPLY_WAIT_TIMEOUT_IN_MS is the timeout). +simx_opmode_oneshot_wait =0x010000 # sends command as one chunk. Reply will also come as one chunk. Waits for the reply (_REPLY_WAIT_TIMEOUT_IN_MS is the timeout). +simx_opmode_continuous =0x020000 +simx_opmode_streaming =0x020000 # sends command as one chunk. Command will be stored on the server and always executed + #(every x ms (as far as possible) where x can be 0-65535. just add x to opmode_continuous). + # A reply will be sent continuously each time as one chunk. Doesn't wait for the reply. + +# Operation modes for heavy data +simx_opmode_oneshot_split =0x030000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_oneshot_split). Reply will also come as several chunks. Doesn't wait for the reply. +simx_opmode_continuous_split =0x040000 +simx_opmode_streaming_split =0x040000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_continuous_split). Command will be stored on the server and always executed. A reply will be sent continuously each time as several chunks. Doesn't wait for the reply. + +# Special operation modes +simx_opmode_discontinue =0x050000 # removes and cancels all commands stored on the client or server side (also continuous commands) +simx_opmode_buffer =0x060000 # doesn't send anything but checks if a reply for the given command is available in the input buffer (i.e. previously received from the server) +simx_opmode_remove =0x070000 # doesn't send anything and doesn't return any specific value. It just erases a similar command reply in the inbox (to free some memory) + + +# Command return codes +simx_return_ok =0x000000 +simx_return_novalue_flag =0x000001 # input buffer doesn't contain the specified command +simx_return_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode +simx_return_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode +simx_return_remote_error_flag =0x000008 # command caused an error on the server side +simx_return_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) +simx_return_local_error_flag =0x000020 # command caused an error on the client side +simx_return_initialize_error_flag =0x000040 # simxStart was not yet called + +# Following for backward compatibility (same as above) +simx_error_noerror =0x000000 +simx_error_novalue_flag =0x000001 # input buffer doesn't contain the specified command +simx_error_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode +simx_error_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode +simx_error_remote_error_flag =0x000008 # command caused an error on the server side +simx_error_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) +simx_error_local_error_flag =0x000020 # command caused an error on the client side +simx_error_initialize_error_flag =0x000040 # simxStart was not yet called + + diff --git a/vrep_scenes/A_square2x2.ttt b/vrep_scenes/A_square2x2.ttt new file mode 100644 index 0000000..2e15a19 Binary files /dev/null and b/vrep_scenes/A_square2x2.ttt differ diff --git a/vrep_scenes/B_maze6x6.ttt b/vrep_scenes/B_maze6x6.ttt new file mode 100644 index 0000000..06fa428 Binary files /dev/null and b/vrep_scenes/B_maze6x6.ttt differ diff --git a/vrep_scenes/C_table_cube.ttt b/vrep_scenes/C_table_cube.ttt new file mode 100644 index 0000000..8c11294 Binary files /dev/null and b/vrep_scenes/C_table_cube.ttt differ diff --git a/vrep_scenes/D_maze4x4_goal.ttt b/vrep_scenes/D_maze4x4_goal.ttt new file mode 100644 index 0000000..7bbc204 Binary files /dev/null and b/vrep_scenes/D_maze4x4_goal.ttt differ