diff --git a/data/xacro_standalone.py b/data/xacro_standalone.py new file mode 100755 index 000000000..a1e77f6f9 --- /dev/null +++ b/data/xacro_standalone.py @@ -0,0 +1,705 @@ +#! /usr/bin/env python +# Copyright (c) 2013, Willow Garage, Inc. +# Copyright (c) 2014, Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Open Source Robotics Foundation, Inc. +# nor the names of its contributors may be used to endorse or promote +# products derived from this software without specific prior +# written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# Author: Stuart Glaser +# Maintainer: William Woodall + +from __future__ import print_function + +import getopt +import glob +import os +import re +import string +import sys +import xml + +from xml.dom.minidom import parse + +try: + _basestr = basestring +except NameError: + _basestr = str + +# Dictionary of subtitution args +substitution_args_context = {} + + +class XacroException(Exception): + pass + + +def isnumber(x): + return hasattr(x, '__int__') + + +# Better pretty printing of xml +# Taken from http://ronrothman.com/public/leftbraned/xml-dom-minidom-toprettyxml-and-silly-whitespace/ +def fixed_writexml(self, writer, indent="", addindent="", newl=""): + # indent = current indentation + # addindent = indentation to add to higher levels + # newl = newline string + writer.write(indent + "<" + self.tagName) + + attrs = self._get_attributes() + a_names = list(attrs.keys()) + a_names.sort() + + for a_name in a_names: + writer.write(" %s=\"" % a_name) + xml.dom.minidom._write_data(writer, attrs[a_name].value) + writer.write("\"") + if self.childNodes: + if len(self.childNodes) == 1 \ + and self.childNodes[0].nodeType == xml.dom.minidom.Node.TEXT_NODE: + writer.write(">") + self.childNodes[0].writexml(writer, "", "", "") + writer.write("%s" % (self.tagName, newl)) + return + writer.write(">%s" % (newl)) + for node in self.childNodes: + # skip whitespace-only text nodes + if node.nodeType == xml.dom.minidom.Node.TEXT_NODE and \ + not node.data.strip(): + continue + node.writexml(writer, indent + addindent, addindent, newl) + writer.write("%s%s" % (indent, self.tagName, newl)) + else: + writer.write("/>%s" % (newl)) +# replace minidom's function with ours +xml.dom.minidom.Element.writexml = fixed_writexml + + +class Table: + def __init__(self, parent=None): + self.parent = parent + self.table = {} + + def __getitem__(self, key): + if key in self.table: + return self.table[key] + elif self.parent: + return self.parent[key] + else: + raise KeyError(key) + + def __setitem__(self, key, value): + self.table[key] = value + + def __contains__(self, key): + return \ + key in self.table or \ + (self.parent and key in self.parent) + + +class QuickLexer(object): + def __init__(self, **res): + self.str = "" + self.top = None + self.res = [] + for k, v in res.items(): + self.__setattr__(k, len(self.res)) + self.res.append(v) + + def lex(self, str): + self.str = str + self.top = None + self.next() + + def peek(self): + return self.top + + def next(self): + result = self.top + self.top = None + for i in range(len(self.res)): + m = re.match(self.res[i], self.str) + if m: + self.top = (i, m.group(0)) + self.str = self.str[m.end():] + break + return result + + +def first_child_element(elt): + c = elt.firstChild + while c: + if c.nodeType == xml.dom.Node.ELEMENT_NODE: + return c + c = c.nextSibling + return None + + +def next_sibling_element(elt): + c = elt.nextSibling + while c: + if c.nodeType == xml.dom.Node.ELEMENT_NODE: + return c + c = c.nextSibling + return None + + +# Pre-order traversal of the elements +def next_element(elt): + child = first_child_element(elt) + if child: + return child + while elt and elt.nodeType == xml.dom.Node.ELEMENT_NODE: + next = next_sibling_element(elt) + if next: + return next + elt = elt.parentNode + return None + + +# Pre-order traversal of all the nodes +def next_node(node): + if node.firstChild: + return node.firstChild + while node: + if node.nextSibling: + return node.nextSibling + node = node.parentNode + return None + + +def child_nodes(elt): + c = elt.firstChild + while c: + yield c + c = c.nextSibling + +all_includes = [] + +# Deprecated message for tags that don't have prepended: +deprecated_include_msg = """DEPRECATED IN HYDRO: + The tag should be prepended with 'xacro' if that is the intended use + of it, such as . Use the following script to fix incorrect + xacro includes: + sed -i 's/ extensions + is_include = False + if elt.tagName == 'xacro:include' or elt.tagName == 'include': + + is_include = True + # Temporary fix for ROS Hydro and the xacro include scope problem + if elt.tagName == 'include': + + # check if there is any element within the tag. mostly we are concerned + # with Gazebo's element, but it could be anything. also, make sure the child + # nodes aren't just a single Text node, which is still considered a deprecated + # instance + if elt.childNodes and not (len(elt.childNodes) == 1 and + elt.childNodes[0].nodeType == elt.TEXT_NODE): + # this is not intended to be a xacro element, so we can ignore it + is_include = False + else: + # throw a deprecated warning + print(deprecated_include_msg, file=sys.stderr) + + # Process current element depending on previous conditions + if is_include: + filename_spec = eval_text(elt.getAttribute('filename'), {}) + if not os.path.isabs(filename_spec): + filename_spec = os.path.join(base_dir, filename_spec) + + if re.search('[*[?]+', filename_spec): + # Globbing behaviour + filenames = sorted(glob.glob(filename_spec)) + if len(filenames) == 0: + print(include_no_matches_msg.format(filename_spec), file=sys.stderr) + else: + # Default behaviour + filenames = [filename_spec] + + for filename in filenames: + global all_includes + all_includes.append(filename) + try: + with open(filename) as f: + try: + included = parse(f) + except Exception as e: + raise XacroException( + "included file \"%s\" generated an error during XML parsing: %s" + % (filename, str(e))) + except IOError as e: + raise XacroException("included file \"%s\" could not be opened: %s" % (filename, str(e))) + + # Replaces the include tag with the elements of the included file + for c in child_nodes(included.documentElement): + elt.parentNode.insertBefore(c.cloneNode(deep=True), elt) + + # Grabs all the declared namespaces of the included document + for name, value in included.documentElement.attributes.items(): + if name.startswith('xmlns:'): + namespaces[name] = value + + elt.parentNode.removeChild(elt) + elt = None + else: + previous = elt + + elt = next_element(previous) + + # Makes sure the final document declares all the namespaces of the included documents. + for k, v in namespaces.items(): + doc.documentElement.setAttribute(k, v) + + +# Returns a dictionary: { macro_name => macro_xml_block } +def grab_macros(doc): + macros = {} + + previous = doc.documentElement + elt = next_element(previous) + while elt: + if elt.tagName == 'macro' or elt.tagName == 'xacro:macro': + name = elt.getAttribute('name') + + macros[name] = elt + macros['xacro:' + name] = elt + + elt.parentNode.removeChild(elt) + elt = None + else: + previous = elt + + elt = next_element(previous) + return macros + + +# Returns a Table of the properties +def grab_properties(doc): + table = Table() + + previous = doc.documentElement + elt = next_element(previous) + while elt: + if elt.tagName == 'property' or elt.tagName == 'xacro:property': + name = elt.getAttribute('name') + value = None + + if elt.hasAttribute('value'): + value = elt.getAttribute('value') + else: + name = '**' + name + value = elt # debug + + bad = string.whitespace + "${}" + has_bad = False + for b in bad: + if b in name: + has_bad = True + break + + if has_bad: + sys.stderr.write('Property names may not have whitespace, ' + + '"{", "}", or "$" : "' + name + '"') + else: + table[name] = value + + elt.parentNode.removeChild(elt) + elt = None + else: + previous = elt + + elt = next_element(previous) + return table + + +def eat_ignore(lex): + while lex.peek() and lex.peek()[0] == lex.IGNORE: + lex.next() + + +def eval_lit(lex, symbols): + eat_ignore(lex) + if lex.peek()[0] == lex.NUMBER: + return float(lex.next()[1]) + if lex.peek()[0] == lex.SYMBOL: + try: + key = lex.next()[1] + value = symbols[key] + except KeyError as ex: + raise XacroException("Property wasn't defined: %s" % str(ex)) + if not (isnumber(value) or isinstance(value, _basestr)): + if value is None: + raise XacroException("Property %s recursively used" % key) + raise XacroException("WTF2") + try: + return int(value) + except: + try: + return float(value) + except: + # prevent infinite recursion + symbols[key] = None + result = eval_text(value, symbols) + # restore old entry + symbols[key] = value + return result + raise XacroException("Bad literal") + + +def eval_factor(lex, symbols): + eat_ignore(lex) + + neg = 1 + if lex.peek()[1] == '-': + lex.next() + neg = -1 + + if lex.peek()[0] in [lex.NUMBER, lex.SYMBOL]: + return neg * eval_lit(lex, symbols) + if lex.peek()[0] == lex.LPAREN: + lex.next() + eat_ignore(lex) + result = eval_expr(lex, symbols) + eat_ignore(lex) + if lex.next()[0] != lex.RPAREN: + raise XacroException("Unmatched left paren") + eat_ignore(lex) + return neg * result + + raise XacroException("Misplaced operator") + + +def eval_term(lex, symbols): + eat_ignore(lex) + + result = 0 + if lex.peek()[0] in [lex.NUMBER, lex.SYMBOL, lex.LPAREN] \ + or lex.peek()[1] == '-': + result = eval_factor(lex, symbols) + + eat_ignore(lex) + while lex.peek() and lex.peek()[1] in ['*', '/']: + op = lex.next()[1] + n = eval_factor(lex, symbols) + + if op == '*': + result = float(result) * float(n) + elif op == '/': + result = float(result) / float(n) + else: + raise XacroException("WTF") + eat_ignore(lex) + return result + + +def eval_expr(lex, symbols): + eat_ignore(lex) + + op = None + if lex.peek()[0] == lex.OP: + op = lex.next()[1] + if not op in ['+', '-']: + raise XacroException("Invalid operation. Must be '+' or '-'") + + result = eval_term(lex, symbols) + if op == '-': + result = -float(result) + + eat_ignore(lex) + while lex.peek() and lex.peek()[1] in ['+', '-']: + op = lex.next()[1] + n = eval_term(lex, symbols) + + if op == '+': + result = float(result) + float(n) + if op == '-': + result = float(result) - float(n) + eat_ignore(lex) + return result + + +def eval_text(text, symbols): + def handle_expr(s): + lex = QuickLexer(IGNORE=r"\s+", + NUMBER=r"(\d+(\.\d*)?|\.\d+)([eE][-+]?\d+)?", + SYMBOL=r"[a-zA-Z_]\w*", + OP=r"[\+\-\*/^]", + LPAREN=r"\(", + RPAREN=r"\)") + lex.lex(s) + return eval_expr(lex, symbols) + + def handle_extension(s): + return ("$(%s)" % s) + + results = [] + lex = QuickLexer(DOLLAR_DOLLAR_BRACE=r"\$\$+\{", + EXPR=r"\$\{[^\}]*\}", + EXTENSION=r"\$\([^\)]*\)", + TEXT=r"([^\$]|\$[^{(]|\$$)+") + lex.lex(text) + while lex.peek(): + if lex.peek()[0] == lex.EXPR: + results.append(handle_expr(lex.next()[1][2:-1])) + elif lex.peek()[0] == lex.EXTENSION: + results.append(handle_extension(lex.next()[1][2:-1])) + elif lex.peek()[0] == lex.TEXT: + results.append(lex.next()[1]) + elif lex.peek()[0] == lex.DOLLAR_DOLLAR_BRACE: + results.append(lex.next()[1][1:]) + return ''.join(map(str, results)) + + +# Expands macros, replaces properties, and evaluates expressions +def eval_all(root, macros, symbols): + # Evaluates the attributes for the root node + for at in root.attributes.items(): + result = eval_text(at[1], symbols) + root.setAttribute(at[0], result) + + previous = root + node = next_node(previous) + while node: + if node.nodeType == xml.dom.Node.ELEMENT_NODE: + if node.tagName in macros: + body = macros[node.tagName].cloneNode(deep=True) + params = body.getAttribute('params').split() + + # Parse default values for any parameters + defaultmap = {} + for param in params[:]: + splitParam = param.split(':=') + + if len(splitParam) == 2: + defaultmap[splitParam[0]] = splitParam[1] + params.remove(param) + params.append(splitParam[0]) + + elif len(splitParam) != 1: + raise XacroException("Invalid parameter definition") + + # Expands the macro + scoped = Table(symbols) + for name, value in node.attributes.items(): + if not name in params: + raise XacroException("Invalid parameter \"%s\" while expanding macro \"%s\"" % + (str(name), str(node.tagName))) + params.remove(name) + scoped[name] = eval_text(value, symbols) + + # Pulls out the block arguments, in order + cloned = node.cloneNode(deep=True) + eval_all(cloned, macros, symbols) + block = cloned.firstChild + for param in params[:]: + if param[0] == '*': + while block and block.nodeType != xml.dom.Node.ELEMENT_NODE: + block = block.nextSibling + if not block: + raise XacroException("Not enough blocks while evaluating macro %s" % str(node.tagName)) + params.remove(param) + scoped[param] = block + block = block.nextSibling + + # Try to load defaults for any remaining non-block parameters + for param in params[:]: + if param[0] != '*' and param in defaultmap: + scoped[param] = defaultmap[param] + params.remove(param) + + if params: + raise XacroException("Parameters [%s] were not set for macro %s" % + (",".join(params), str(node.tagName))) + eval_all(body, macros, scoped) + + # Replaces the macro node with the expansion + for e in list(child_nodes(body)): # Ew + node.parentNode.insertBefore(e, node) + node.parentNode.removeChild(node) + + node = None + elif node.tagName == 'arg' or node.tagName == 'xacro:arg': + name = node.getAttribute('name') + if not name: + raise XacroException("Argument name missing") + default = node.getAttribute('default') + if default and name not in substitution_args_context['arg']: + substitution_args_context['arg'][name] = default + + node.parentNode.removeChild(node) + node = None + + elif node.tagName == 'insert_block' or node.tagName == 'xacro:insert_block': + name = node.getAttribute('name') + + if ("**" + name) in symbols: + # Multi-block + block = symbols['**' + name] + + for e in list(child_nodes(block)): + node.parentNode.insertBefore(e.cloneNode(deep=True), node) + node.parentNode.removeChild(node) + elif ("*" + name) in symbols: + # Single block + block = symbols['*' + name] + + node.parentNode.insertBefore(block.cloneNode(deep=True), node) + node.parentNode.removeChild(node) + else: + raise XacroException("Block \"%s\" was never declared" % name) + + node = None + elif node.tagName in ['if', 'xacro:if', 'unless', 'xacro:unless']: + value = eval_text(node.getAttribute('value'), symbols) + try: + if value == 'true': keep = True + elif value == 'false': keep = False + else: keep = float(value) + except ValueError: + raise XacroException("Xacro conditional evaluated to \"%s\". Acceptable evaluations are one of [\"1\",\"true\",\"0\",\"false\"]" % value) + if node.tagName in ['unless', 'xacro:unless']: keep = not keep + if keep: + for e in list(child_nodes(node)): + node.parentNode.insertBefore(e.cloneNode(deep=True), node) + + node.parentNode.removeChild(node) + else: + # Evals the attributes + for at in node.attributes.items(): + result = eval_text(at[1], symbols) + node.setAttribute(at[0], result) + previous = node + elif node.nodeType == xml.dom.Node.TEXT_NODE: + node.data = eval_text(node.data, symbols) + previous = node + else: + previous = node + + node = next_node(previous) + return macros + + +# Expands everything except includes +def eval_self_contained(doc): + macros = grab_macros(doc) + symbols = grab_properties(doc) + eval_all(doc.documentElement, macros, symbols) + + +def print_usage(exit_code=0): + print("Usage: %s [-o ] " % 'xacro.py') + print(" %s --deps Prints dependencies" % 'xacro.py') + print(" %s --includes Only evalutes includes" % 'xacro.py') + sys.exit(exit_code) + + +def set_substitution_args_context(context={}): + substitution_args_context['arg'] = context + +def open_output(output_filename): + if output_filename is None: + return sys.stdout + else: + return open(output_filename, 'w') + +def main(): + try: + opts, args = getopt.gnu_getopt(sys.argv[1:], "ho:", ['deps', 'includes']) + except getopt.GetoptError as err: + print(str(err)) + print_usage(2) + + just_deps = False + just_includes = False + + output_filename = None + for o, a in opts: + if o == '-h': + print_usage(0) + elif o == '-o': + output_filename = a + elif o == '--deps': + just_deps = True + elif o == '--includes': + just_includes = True + + if len(args) < 1: + print("No input given") + print_usage(2) + + # Process substitution args + # set_substitution_args_context(load_mappings(sys.argv)) + set_substitution_args_context((sys.argv)) + + f = open(args[0]) + doc = None + try: + doc = parse(f) + except xml.parsers.expat.ExpatError: + sys.stderr.write("Expat parsing error. Check that:\n") + sys.stderr.write(" - Your XML is correctly formed\n") + sys.stderr.write(" - You have the xacro xmlns declaration: " + + "xmlns:xacro=\"http://www.ros.org/wiki/xacro\"\n") + sys.stderr.write("\n") + raise + finally: + f.close() + + process_includes(doc, os.path.dirname(args[0])) + if just_deps: + for inc in all_includes: + sys.stdout.write(inc + " ") + sys.stdout.write("\n") + elif just_includes: + doc.writexml(open_output(output_filename)) + print() + else: + eval_self_contained(doc) + banner = [xml.dom.minidom.Comment(c) for c in + [" %s " % ('=' * 83), + " | This document was autogenerated by xacro from %-30s | " % args[0], + " | EDITING THIS FILE BY HAND IS NOT RECOMMENDED %-30s | " % "", + " %s " % ('=' * 83)]] + first = doc.firstChild + for comment in banner: + doc.insertBefore(comment, first) + + open_output(output_filename).write(doc.toprettyxml(indent=' ')) + print() + +if __name__ == '__main__': + main() diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index db3040c7d..644581976 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -132,7 +132,7 @@ bool PhysicsClientSharedMemory::processServerStatus(ServerStatus& serverStatus) { btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1); - const SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; hasStatus = true; serverStatus = serverCmd; @@ -272,7 +272,7 @@ bool PhysicsClientSharedMemory::processServerStatus(ServerStatus& serverStatus) case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { b3Printf("Received actual state\n"); - SharedMemoryCommand& command = m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0]; int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ; int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU; @@ -432,6 +432,26 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c break; } + + case CMD_INIT_POSE: + { + if (m_data->m_serverLoadUrdfOK) + { + b3Printf("Initialize Pose"); + m_data->m_testBlock1->m_clientCommands[0] = command; + m_data->m_testBlock1->m_numClientCommands++; + + } + break; + } + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + b3Printf("Send Physics Simulation Parameters"); + m_data->m_testBlock1->m_clientCommands[0] = command; + m_data->m_testBlock1->m_numClientCommands++; + break; + } + case CMD_REQUEST_ACTUAL_STATE: { if (m_data->m_serverLoadUrdfOK) diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index a2ba50be4..7f6c77bb9 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -316,7 +316,7 @@ void PhysicsServerSharedMemory::processClientCommands() m_data->m_worldImporters.push_back(worldImporter); bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); - SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; if (completedOk) { @@ -345,7 +345,7 @@ void PhysicsServerSharedMemory::processClientCommands() urdfArgs.m_initialOrientation[2], urdfArgs.m_initialOrientation[3]), urdfArgs.m_useMultiBody, urdfArgs.m_useFixedBase); - SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; if (completedOk) { @@ -450,7 +450,7 @@ void PhysicsServerSharedMemory::processClientCommands() - SharedMemoryCommand& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; serverCmd.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; m_data->m_testBlock1->m_numServerCommands++; break; @@ -461,7 +461,7 @@ void PhysicsServerSharedMemory::processClientCommands() if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); - SharedMemoryCommand& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0; @@ -553,13 +553,49 @@ void PhysicsServerSharedMemory::processClientCommands() b3Printf("Step simulation request"); m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime); - SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; m_data->m_testBlock1->m_numServerCommands++; break; } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + if (clientCmd.m_physSimParamArgs.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) + { + btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); + this->m_data->m_dynamicsWorld->setGravity(grav); + b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); + + } + + + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; + m_data->m_testBlock1->m_numServerCommands++; + + + break; + + }; + case CMD_INIT_POSE: + { + b3Printf("Server Init Pose not implemented yet"); + ///@todo: implement this + m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); + + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; + m_data->m_testBlock1->m_numServerCommands++; + break; + } + + + case CMD_SHUTDOWN: { btAssert(0); @@ -581,7 +617,7 @@ void PhysicsServerSharedMemory::processClientCommands() bool isDynamic = (mass>0); worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; + SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; m_data->m_testBlock1->m_numServerCommands++; break; diff --git a/examples/SharedMemory/RobotControlExample.cpp b/examples/SharedMemory/RobotControlExample.cpp index b38f0a0ef..73d757816 100644 --- a/examples/SharedMemory/RobotControlExample.cpp +++ b/examples/SharedMemory/RobotControlExample.cpp @@ -97,6 +97,27 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr) cl->enqueueCommand(command); break; } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS; + command.m_physSimParamArgs.m_gravityAcceleration[0] = 0; + command.m_physSimParamArgs.m_gravityAcceleration[1] = 0; + command.m_physSimParamArgs.m_gravityAcceleration[2] = -10; + command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY; + cl->enqueueCommand(command); + break; + + }; + case CMD_INIT_POSE: + { + ///@todo: implement this + command.m_type = CMD_INIT_POSE; + cl->enqueueCommand(command); + break; + } + + case CMD_CREATE_BOX_COLLISION_SHAPE: { command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE; @@ -225,7 +246,8 @@ void RobotControlExample::initPhysics() createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger); - + createButton("Set Physics Params",CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,isTrigger); + createButton("Init Pose",CMD_INIT_POSE,isTrigger); } else { /* diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 7299d692b..a2921e69c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -22,10 +22,28 @@ typedef unsigned long long int smUint64_t; #endif -enum SharedMemoryServerCommand +enum EnumSharedMemoryClientCommand { - CMD_URDF_LOADING_COMPLETED, - CMD_URDF_LOADING_FAILED, + CMD_LOAD_URDF, + CMD_SEND_BULLET_DATA_STREAM, + CMD_CREATE_BOX_COLLISION_SHAPE, +// CMD_DELETE_BOX_COLLISION_SHAPE, +// CMD_CREATE_RIGID_BODY, +// CMD_DELETE_RIGID_BODY, +// CMD_SET_JOINT_FEEDBACK,///enable or disable joint feedback for force/torque sensors + CMD_INIT_POSE, + CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, + CMD_SEND_DESIRED_STATE, + CMD_REQUEST_ACTUAL_STATE, + CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE + CMD_SHUTDOWN, + CMD_MAX_CLIENT_COMMANDS +}; + +enum EnumSharedMemoryServerStatus +{ + CMD_URDF_LOADING_COMPLETED, + CMD_URDF_LOADING_FAILED, CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, CMD_BULLET_DATA_STREAM_RECEIVED_FAILED, CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED, @@ -33,24 +51,8 @@ enum SharedMemoryServerCommand CMD_SET_JOINT_FEEDBACK_COMPLETED, CMD_ACTUAL_STATE_UPDATE_COMPLETED, CMD_DESIRED_STATE_RECEIVED_COMPLETED, - CMD_STEP_FORWARD_SIMULATION_COMPLETED, - CMD_MAX_SERVER_COMMANDS -}; - -enum SharedMemoryClientCommand -{ - CMD_LOAD_URDF, - CMD_SEND_BULLET_DATA_STREAM, - CMD_CREATE_BOX_COLLISION_SHAPE, - CMD_DELETE_BOX_COLLISION_SHAPE, - CMD_CREATE_RIGID_BODY, - CMD_DELETE_RIGID_BODY, - CMD_SET_JOINT_FEEDBACK,///enable or disable joint feedback - CMD_SEND_DESIRED_STATE, - CMD_REQUEST_ACTUAL_STATE, - CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE - CMD_SHUTDOWN, - CMD_MAX_CLIENT_COMMANDS + CMD_STEP_FORWARD_SIMULATION_COMPLETED, + CMD_MAX_SERVER_COMMANDS }; #define SHARED_MEMORY_SERVER_TEST_C @@ -89,6 +91,19 @@ enum { CONTROL_MODE_TORQUE, }; +///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position +///No motors or controls are needed to initialize the pose. It is similar to +///moving a robot to a starting place, while it is switched off. It is only called +///at the start of a robot control session. All velocities and control forces are cleared to zero. +struct InitPoseArgs +{ + int m_bodyUniqueId; + double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; +}; + + +///Controlling a robot involves sending the desired state to its joint motor controllers. +///The control mode determines the state variables used for motor control. struct SendDesiredStateArgs { int m_bodyUniqueId; @@ -104,6 +119,29 @@ struct SendDesiredStateArgs }; + +enum EnumUpdateFlags +{ + SIM_PARAM_UPDATE_DELTA_TIME=1, + SIM_PARAM_UPDATE_GRAVITY=2, + SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4, + SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8, +}; + +///Controlling a robot involves sending the desired state to its joint motor controllers. +///The control mode determines the state variables used for motor control. +struct SendPhysicsSimulationParameters +{ + //a bit fields to tell which parameters need updating, see SIM_PARAM_UPDATE_DELTA_TIME etc. + //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; + int m_updateFlags; + + double m_deltaTime; + double m_gravityAcceleration[3]; + int m_numSimulationSubSteps; + int m_numSolverIterations; +}; + struct RequestActualStateArgs { int m_bodyUniqueId; @@ -138,13 +176,29 @@ struct SharedMemoryCommand union { UrdfArgs m_urdfArguments; + InitPoseArgs m_initPoseArgs; + SendPhysicsSimulationParameters m_physSimParamArgs; BulletDataStreamArgs m_dataStreamArguments; SendDesiredStateArgs m_sendDesiredStateCommandArgument; RequestActualStateArgs m_requestActualStateInformationCommandArgument; - SendActualStateArgs m_sendActualStateArgs; }; }; -typedef SharedMemoryCommand ServerStatus; + +struct SharedMemoryStatus +{ + int m_type; + + smUint64_t m_timeStamp; + int m_sequenceNumber; + + union + { + BulletDataStreamArgs m_dataStreamArguments; + SendActualStateArgs m_sendActualStateArgs; + }; +}; + +typedef SharedMemoryStatus ServerStatus; #endif //SHARED_MEMORY_COMMANDS_H diff --git a/examples/SharedMemory/SharedMemoryInterface.h b/examples/SharedMemory/SharedMemoryInterface.h index 43af44496..084559451 100644 --- a/examples/SharedMemory/SharedMemoryInterface.h +++ b/examples/SharedMemory/SharedMemoryInterface.h @@ -12,7 +12,7 @@ struct SharedMemoryExampleData { int m_magicId; SharedMemoryCommand m_clientCommands[SHARED_MEMORY_MAX_COMMANDS]; - SharedMemoryCommand m_serverCommands[SHARED_MEMORY_MAX_COMMANDS]; + SharedMemoryStatus m_serverCommands[SHARED_MEMORY_MAX_COMMANDS]; int m_numClientCommands; int m_numProcessedClientCommands;