Home

User`s Manual - Electrical Engineering and Computer Science

1. Fell k Fr otherwise F 0 if Fi lt ky En f 0 39 O 200 ace 240 z B10 Z 100 g ay Tns 3 0 E 10 F 5 AQ z los 100 10 F Ba wa 200 10 peii ce eee 1 0 5 0 0 5 10 10 10 Shaft speed rev min x10 i Discharge Current A a b Figure 7 a Torque speed curve for the Maron RE118751 20W DC motor b Battery discharge curve for the Panasonic 12V 2 2A battery where F and F are the components of F tangent and normal to the terrain surface respectively and ky is the surface friction coefficient Using these equations the vector field for each toe is defined based on the mode of the corresponding leg 5 1 6 Actuator Model In the compliant hexapod we also incorporate a simple model of the hip actuation This model imposes realistic limitations on the torque capabilities of the actuators Figure 7 a portrays the torque speed curve for a Maxon RE118751 DC motor This curve characterizes the maximum torque deliverable by the motor as a function of shaft speed In addition to these torque limits the model also incorporates an estimation scheme for motor voltages vg and currents ta using the commanded leg torques under the assump tion that the electrical dynamics are negligible relative to the mechanical dynamics In consequence we have la T Krkg taPa Kwwe kg Va where K and K are motor constants kg is the gear reduction ratio of
2. goar ratio torque_loss Actuator efficiency unused rho0_k rho5_k 500 1000 500 Radial leg spring constants N m 500 1000 500 rho0_d rho5_d 10 20 10 10 Radial leg damping constants Ns m 20 10 rho0_r0 rho5_r0 0 2 0 2 0 2 Radial leg spring rest lengths m 0 2 0 2 0 2 theta0_k theta5_k 100 200 100 Angular leg spring constants Nm rad 100 200 100 theta0_d theta5_d 0 4 0 8 0 4 Angular leg damping constants 0 4 0 8 0 4 Nms rad theta0_r0 theta5_r0 0 2 0 2 0 2 Angular leg spring rest lengths rad 0 2 0 2 0 2 attachO_x attach0_y 0 1 0 2 0 Leg 0 attachment point coordinates m attach0_z ee attachi_x attachi _y 0 1 0 0 Leg 1 attachment point coordinates m attachi_z eee gt S gt Se ARS attach2_x attach2_y 0 1 0 2 0 Leg 2 attachment point coordinates m attach3_x attach3_y 0 1 0 2 0 Leg 3 attachment point coordinates m attach4_x attach4_y 0 1 0 2 0 Leg 4 attachment point coordinates m attach5 x attach5_y 0 1 0 2 0 Leg 5 attachment point coordinates m continued on next page 46 terrain_type Height function Magy 0 h x a sin a2x cos azy 1y R a x a2y a3 y gt 0 sloped h z y a3 otherwise Table 3 Height functions for different types of terrain a a2 and a3 correspond to terrain_argl terrain_arg2 and terrain_arg3 respectively continued from previous page J11 J 12
3. When defining a new controller the user must provide functions for each of these components and modify SS_HexControl c accordingly This structure gives the user the flexibility to define separate charts and vector fields for the controller itself independent of the structure of the hexapod However the current implementation of the integration engine requires the state space and the chart structure to be unique for the overall system which necessitates some care in the controller implementa tion The bit fields in the chart number associated with the hexapod model and the vector field elements of the hexapod should not be modified in the controller functions unless it is explicitly required for controller functionality Currently the only controller that is implemented is the open loop controller described in Section 5 2 control_type open loop Its implementation is done in SS_OpenLoop c 51 5 5 The Implementation of the Open Loop Controller 5 5 1 Controller Vector Field The SimSect implementation of the open loop controller introduces six new states corre sponding to the reference trajectories for each leg The vector fields associated with each of these states are determined based on the current controller chart which for each leg encodes the current phase of the motion profile of Figure 8 The fields SS_OL legSpeed leg_no hold the vector fields for each of the legs and are computed at each controller chart transition bas
4. x 4 legy x 1 x 5 val Q_RL sqrt legx legx legy legy break k default SS_FatalError slip_Boundary INVALID_PARAM_ERROR return INVALID_PARAM_ERROR return NO_ERROR 4 3 8 The Transition Function The transition function determines the new state and chart after a boundary crossing In the SLIP model there is only one possible cyclic chart sequence ascent descent compres sion decompression which is implemented by the transition function The state of the system does not change except during a transition from descent to compression touchdown transition and a transition from decompression into ascent liftoff transition The former transition sets the states fy and fy to their new values after the foot touches the ground The latter transition however sets the state qg to the negative of the liftoff angle to preserve neutrally stable symmetric SLIP orbits int slip_Transition int trans int newChart double xNew double x double pL int chart SS_StopFunc sf 29 memcpy xNew x NUM_OF_STATES sizeof double SS_GetStopFunc amp sf 1 if sf state STOP_FUNC_CROSSING switch chart case ASCENT_CHART newChart DESCENT_CHART SS_Message Transition ASCENT gt DESCENT n break case DESCENT_CHART xNew 4 xNew 5 x 0 Q_RT sin x 6 x 1 Q_RT cos x 6 xnewChart COMPRESSION_CHART SS_Message Transition DESCENT gt COMPRESSION
5. 33 1 at the motor shaft and r is the motor armature resistance Note that in this simple formulation the only influence of the actuator model on the mechanical dynamics is through the limits on the maximum available torque In fact va and ig are not computed by SimSect but are extracted later from the simulation data 40 5 1 7 Battery Model The discharge characteristics of off the shelf small batteries are usually given by plots of discharge time vs constant discharge current Figure 7 b is such a discharge curve for the Panasonic 12V 2 2Ah lead acid battery used In our discharge model we use this curve together with an approximation of the PWM electronics driving the DC motors to estimate the duration of autonomous operation First we derive a method for estimating battery discharge in terms of a continuous time function of varying discharge current This can be accomplished with dct 1 dt Filt where C t is the percent energy left in the battery and f i is the battery discharge curve in functional form During the hexapod operation all six motors draw current and contribute to the battery discharge together Due to the H bridge output stages of the motor drives the motor currents add up yielding the battery lifetime equation 1 4m o ilo Our battery model detects in simulation the zero crossing of this function which yields the effective lifetime of the battery Note that this model does not take into
6. J_13 J_21 J_22 J_23 J 31 J 32 J_33 terrain_type terrain_arg1 terrain_arg2 terrain_arg3 frame_width frame_height 5 4 2 Terrain Surface The hexapod model implementation currently supports three types of terrain The configu ration parameter terrain_type selects the surface to be used in the simulation There are also three other parameters modulating the way the currently selected terran is generated The functions terrain_height and terrain_normal in SS_HexTerrain c defines the height and surface normal functions for different types of terrain 47 5 4 3 Initializing the Partition The function hex_InitPartition is called during system initialization and sets up the initial state of the model In the hexapod model its main function is to determine which legs are in stance and which legs are in flight based on the state of the body and assuming an alternating tripod posture as the initial conditions This is accomplished by looking at the rest states of the legs at an alternate tripod posture and determining which legs should be touching the ground based on their toe positions The initialization also involves the setting up of the visualization subsystem by load ing the appropriate geometry file into Geomview and sending the necessary initialialization commands 5 4 4 Defining the Chart The function hex_DefineChart performs the definition of a new chart Following a transition the trajectory iterator removes all
7. Kutta integration algorithm for a single time step The iteration accomplishes the refinement of the time step until the state error falls below a certain tolerance determining the time step for computing the next trajectory point This module consists of the files SS_RungeKutta c and SS_RungeKutta h The list below summarizes the functions exported by this module e SS_InitRK Sets up a Runge Kutta iterator with appropriate system states and parameters e SS_UpdateRK Updates the current chart for the trajectory computation e SS_RunRK Carries out the iteration and computes the next trajectory point as well as updating the time step e SS_RK_NextStep Provides external access to the iteration function without going through the whole iteration process with wrapup etc 3 3 3 The Flow Iterator The flow iterator is the module which repeatedly calls SS_RunRK to compute the trajectory inside a particular chart until a boundary crossing is detected The functionality of the flow computation module is detailed in Figure 3 Given an initial condition this module iterates through successive trajectory points using the Runge Kutta module to advance through time steps Upon detection of a boundary crossing the stopping iterator is invoked refining the last time step until the boundary crossing time is determined up to the required precision This module is implemented by the files SS_ComputeFlow c and SS_ComputeFlow h
8. Note that the function spring_derivative defines the derivative of the potential function defined in 1 and hence gives the negative of the spring force static double spring_derivative double qr double qr0 double k int i int j if i 2 amp amp j 1 return k qr qr0 return k i fabs i fabs j pow pow qr j pow qr0 j j fabs j i 1 fabs j pow qr j 1 int slip_VectorField double xdot double x double p int chart if chart ASCENT_CHART chart DESCENT_CHART xdot 0 x 2 xdot 1 x 3 xdot 2 0 0 xdot 3 GFLIGHT xdot 4 xdot 5 0 0 else if chart COMPRESSION_CHART chart DECOMPRESSION_CHART double legx x 0 x 4 double legy x 1 x 5 double theta atan2 legx legy double rho sqrt legx legx legy legy double force force spring_derivative rho SPR_REST SPR_K SPR_I SPR_J xdot 0 x 2 xdot 1 x 3 xdot 2 force xsin theta M_BODY xdot 3 force cos theta M_BODY GSTANCE xdot 4 xdot 5 0 0 else SS_FatalError slip_VectorField INVALID_PARAM_ERROR return INVALID_PARAM_ERROR return NO_ERROR 27 4 3 7 Defining the Charts After the partition is initialzed and after each chart transition the slip_DefineChart function is called Depending on what the next chart is this function creates the necessary stopping functions In the SLIP model each chart has only one associated stopping
9. The following functions provide access to the functionality of this module e SS_InitFlow Initializes necessary structures for the flow iterator e SS_UpdateFlow Updates the current chart for a flow iterator e SS_PurgeFlow Purges memory structures allocated by the InitFlow function for a flow iterator flow_iterate integ_iterate integ_wrapup stop_iterate Figure 3 Flowcharts for major functions in the SS_ComputeTraj SS_ComputeFlow and SS_Stopping modules The flow_iterate is the iteration function for the trajectory iterator The integ_integrate and integ_wrapup are the iteration and wrapup functions of the flow iterator and stop_iterate is the iteration function for the stopping function iterator e SS_IntegFlow Integrates the dynamical system model in the current chart until a boundary crossing is detected by invoking the flow iterator Then it uses the stopping iterator to detect the exact crossing time and stops the integration at th
10. accomplished by updating the transformation matrices associated with the geometry in a way that is particular to the model and the design choices of the model programmer The functions in the files SS_Geomview c and SS_Geomview h implement the visualization utilities of SimSect The following functions are provided by this module e SS_GeomviewInit Initializes the visualization subsystem If the symbol useGeomview is set to 0 in the initialization file the default is 1 then this function returns without doing anything Otherwise it attempts to execute Geomview or establish connection to an existing Geomview execution If successful it resets Geomview and prepares it for the upcoming visualization tasks This function is called from the SimSect top level execution e SS_GeomviewSend Sends a string to Geomview which should be in Geomview Command Language gcl 15 e SS_GeomviewWait Waits for Geomview to finish whatever it is currently doing and then returns e SS_GeomviewTransform Sends a number of transformation matrices to Geomview redefining existing trans formation matrices with names tttO ttt1 ttt2 This function is used by the model definition to update the state of the visual image based on the current state Essentially this operation updates the visual reflecting the new positions and orientations of the geometries which depend on the redefined transformation matrices e SS_GeomviewSendFil
11. boundaries in the form of zero level sets of scalar functions and the transition functions which map the system state on one chart to the system state on the following chart in the case of a boundary crossing For a more detailed discussion on the hybrid system concepts used in SimSect refer to Section 3 1 2 Using SimSect 2 1 System Requirements SimSect currently runs on Intel x86 based Linux platforms Visualization of the simulation requires Geomview available from http www geomview org 2 2 Invocation SimSect consists of a single executable SimSect It accepts only one argument which spec ifies the initialization file through which most of the system configuration is done The default name for the initialization file is SimSect rc Usage SimSect c filename 2 3 Output Files Upon completion of execution SimSect creates three output files The main output file is SimSect data which includes an ascii dump of the auxiliary functions defined by the model By default the data in this file is collected at every integration time step The sampling period can be redefined in the initialization file to avoid very large data files The SimSect initial and the SimSect param files include the model initial conditions and the model parameters respectively These two files have the same format as the initialization file see Section 2 4 1 2 4 The Initialization File System configuration and the specification of model parameter
12. error state to stop the integration 48 5 4 6 The Vector Field The function hex_VectorField defines the vector field for the compliant hexapod model on the state space defined in Section 5 3 1 Its definition is included in SS_HexVectorField c However SS_HexForces c and SS_HexTransform define many of the coordinate transforma tions and computations of the components for the vector field Please refer to the source code for the details The vector field associated with the first half of the state space specifically Rp Wp rp fo is that of rigid body dynamics and was derived in Section 5 1 4 Note that the computation of the leg forces uses all the state variables including the foot locations The second half of the state space consists of the positions and velocities of the toes The vector fields associated with these are defined in Section 5 1 5 5 4 7 Visualization The hexapod model exploits the interconnection of SimSect to Geomview When the sub system is on it visualizes a three dimensional model of the hexapod as it is locomoting over the specified terrain The file hexaped oogl1 includes the initial definition of the hexapod geometry together with the rigid body a flat ground all six legs the feet and all the associated transformation matrices This file is loaded into Geomview during initialization for the creation of all objects necessary for the real time update of the scene The file SS_HexGeomview c implements th
13. field for the individual charts e Define the properties of a particular chart and determine the boundary functions that will be used defining stopping functions as necessary 16 e Compute boundary functions identified by a certain index that the chart initialization determines for the current chart e Perform chart transitions by computing the next system state and chart e Validate a chart by checking whether a given trajectory point lies in the chart e Compute an auxiliary function mainly used for data collection purposes e Compute the homogeneous transformations for visualization of the system trajectory using Geomview 3 5 1 The State Space Structure One of the first things that the model programmer needs to determine is the structure of the state space in each of the charts of the system model In SimSect there are two different types of state space variables that are commonly used The first type corresponds to the usual continuous state variables and by convention occupy the topmost part of the state space i e states 0 1 2 Note that each chart may have a different number of these continuous states among which the conversion will be carried out by the transition function The second type of states are all other components of the model that require memory These are mainly bookkeeping states which change with transitions between different charts Due to the fact that the vector field definition is unable to retai
14. for storing the auxiliary function data e Initialize the flow memory for storing the state trajectory data e Call the InitPartition function of the model to setup the initial condition for the integration as well as any other initializations that the model may want to execute e Initialize the trajectory integrator e Save the initial conditions and the model parameters into appropriate output files e Integrate the model system until the trajectory iterator exits which is either at the final integration time or some error has occurred e Save the trajectory data and auxiliary functions into appropriate output file cleanup and exit 3 4 Utility Modules 3 4 1 The Flow Memory Utility Module In SimSect the data points resulting from the integration of the model either the auxiliary functions or the state variables are stored in a certain data structure called flow memory SimSect provides facilities to create maintain and delete flow memories making it possible to collect and save data in a convenient way The files SS_FlowMem c and SS_FlowMem h implement the functions associated with this utility module The provided functions and their descriptions are summarized below e SS_FlowMemInit Allocates and initializes a flow memory structure to be used by the other utility func tions in this module An uninitialized flow memory object cannot be used by the utility functions Note that the number of elements doubles in a data en
15. function in addition to the system defined final time and measurement stopping functions which correspond to the boundary functions defined in Section 4 2 int slip_DefineChart SS_ComputeFlow cf int chart int errno SS_StopFunc sf sf type STOP_TYPE_RIGHT sf action slip_Boundary if chart ASCENT_CHART sf index 0 else if chart DESCENT_CHART sf index 1 else if chart COMPRESSION_CHART sf index 2 else if chart DECOMPRESSION_CHART sf index 3 else SS_FatalError slip_Transition INVALID_PARAM_ERROR return INVALID_PARAM_ERROR if errno SS_AddStopFunc cf amp sf lt 0 return errno return NO_ERROR Note that the index field in the stopping function structure determines which boundary function is evaluated by that particular stopping function The boundary function are defined with the following piece of code int slip_Boundary double val double x double p int chart int index switch index case 0 The ascent descent boundary val x 3 break 28 case 1 The descent compression boundary val x 1 Q_RT cos x 6 break case 2 The compression decompression boundary double legx x 0 x 4 legy x 1 x 5 double xdot x 2 ydot x 3 val legx xdot legy ydot sqrt legx legx legy legy break case 3 The decompression ascent boundary double legx x 0
16. recordings of the system trajectory at a given frequency e measurePeriod default 0 0 The time period for displaying a user message at particular time intervals This is a convenient way of interactively displaying the integration progress However a very small value for this symbol might result in excessive output of messages e maxChartCount default 128 The maximum number of times chart transitions can occur In cases where vector fields in neighboring charts point towards each other chattering may occur which usually throws the integrator into a state where chart transitions occur at very high frequencies When the number of transitions exceed this value the integration stops with an error and the results of the integration up to this point are saved e finalTime default 1 0 This is the final time for the integration Upon reaching this point in time the inte gration stops and the computed trajectory is saved in the data files The integrator actually defines a specific stopping function for this purpose so the final integration time corresponds to this value up to the stopping precision e tolerance default 1e 4 This is the state error tolerance setting of the Runge Kutta integrator e maxTimeStep default 1e 2 The maximum time step limit for the integration The time step is constrained to be below this value by the Runge Kutta algorithm for numerical stability purposes This value in conjunction with th
17. refer to the source code for the details of the algorithm which is a very simple root finder The files associated with this module are SS_RootFinder c and SS_RootFinder h The function provided by the module are summarized below e SS_InitRootFinder Initializes the specified root finder object e SS_FindRoot Finds a zero of the function specified in the arguments closest to a starting point supplied in another argument 3 4 5 Visualization Utility Module SimSect implements an interface with Geomview a three dimensional graphics package for the visualizing the simulation of the model This interface is real time in the sense that the visual is updated as the simulation progresses Currently there is no off line visualization option SimSect interfaces with Geomview through named Unix pipes through which it uses Geomview s command language to control its behavior If the symbol useGeomview is set to 1 in the initialization file SimSect invokes Geomview and establishes a pipe connection for communication If the attempt to execute and connect to Geomview fails the subsystem is disabled SimSect assumes a particular way in which the model state is visualized The model definition must provide functions to initialize the initial three dimensional geometry together with appropriate transformation matrices see 3 for a detailed account of how to construct geometries in Geomview The real time visualization is then
18. the collection of charts and patches is called an atlas For each a J there is a finite set of boundary functions hai Va gt R i J f and real numbers called target values Cai for i JP that satisfy the condition For x V where a I we require x U if and only if hai x Co gt 0 for alli e JP Thus a patch is to be considered the domain on which a collection of smooth functions are positive The boundary of a patch is assumed to lie within the set J has Cait frac ic J bt Conceptually the evolution of the system is viewed as a sequence of trajectory segments where the endpoint of one segment is connected to the initial point of the next by a transformation It follows that time may be divided into contigu ous periods called epochs separated by instances where transition functions are applied at times referred to as events The transition functions are maps which send a point on the boundary of one patch to a point in another not necessarily different patch in the atlas Within this framework an orbit in the flow of a hybrid dynamical system which begins at a time t and terminates at tp may be completely described A trajectory hence is a curve y to tr V xI together with an increasing sequence of real numbers to lt t lt lt tm ty that satisfies three properties e Each time interval t ti 1 corresponds to an epoch and there exists a des ignated a so that q t lies ent
19. zero crossings are detected and iteratively identified up to a certain precision by the stopping iterator These zero crossings may occur either when a boundary crossing occurs or when a system defined condition such as the final integration time is encountered The return value is an index which will later be used to access the state of the added stopping function e SS_ClearStopFunc Clears all currently active stopping functions Usually called at the end of each chart transition in which case the model defines new boundary functions e SS_GetStopFunc Returns the state of the boundary function The model definition frequently uses this function to check which boundary function initiated the chart transition to decide on the proper course of action 3 3 5 The Trajectory Iterator The trajectory iterator module is one level higher than the flow iterator and is responsible from iterating through successive charts Each execution of the iteration corresponds to the invocation of a complete flow iteration resulting in the computation of the trajectory from the current state until the next boundary crossing Figure 3 gives the flowchart for the main iteration loop for this module Note that this module is also responsible from defining the final time stopping function as well as calling various components of the model to initialize the new partition structure The function associated with this module are located in the files SS_Com
20. 2 we derive these forces and torques for a single generalized leg leading to the formulation of the system dynamics in Section 5 1 4 5 1 2 Analysis of a Single Leg Our formulation of the equations of motion for the hexapod model individually computes the ground reaction forces for each leg To this end it suffices to analyze a generic leg parametrized by its attachment and touchdown points a and fj respectively see Figure 6 The force and torque balance on the massless leg result in the following equalities 36 Coordinate Frames Tnertial world frame Bo body frame leg attachment point in B Se 9 States x i Rp Wp rb th fi fi Forces and Torques Ta Motor speed and torque constants Table 1 Notation used for the Compliant Hexapod model 37 Figure 6 Analysis of a single leg in the plane defined by the leg and the z axis of B Ffi Fy Fo 1 Pi Toi F i pi cos 0 The body experiences the negative of the ground reaction force on the leg yielding effective force and torque vectors acting on the center of mass Projection of these on B for each leg 7 1 6 yields sin 6 cos 6 0 F cos6 sing sind sing cos d cos cos sin6 cos sin d Fr To Pi T4 pi 60804 R Vi a x F Note that in the equations above one also needs to specify the spring and damper models for the radial and lateral angular degrees of freedom for each leg In the
21. SimSect Hybrid Dynamical Simulation Environment User s Manual Uluc Saranli Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor MI 48109 2110 USA ulucs eecs umich edu Abstract This report is the user s manual of SimSect a flexible environment for the sim ulation of hybrid dynamical systems Hybrid dynamical systems are systems where the continuous dynamics undergo discrete changes upon detection of predefined state based events The simulation of such systems involve accurate detection of events and handling of the transition between different continuous systems This report also includes the details of two example hybrid dynamical systems A spring loaded inverted pendulum SLIP runner and a compliant hexapod The former example illustrates the basic elements of programming models with SimSect while the latter implements a much more complicated model addressing many common issues arising when defining complex dynamical models with SimSect 1 Introduction 1 1 What is SimSect SimSect is a general purpose hybrid dynamical system integration environment It was mainly built to simulate a hexapod robot with compliant legs It can however be used to define arbitrary hybrid dynamical system systems with piecewise continuous dynamics and numerically compute their state trajectories from arbitrary initial conditions It also incorpo rates an interface to Geomview a 3D visualizatio
22. UpdateGeomview double x double p int chart double f 256 int num SS_GeomviewSend amp SS_Data geomview freeze Camera num Transform f x p chart SS_GeomviewTransform amp SS_Data geomview num f SS_GeomviewSend amp SS_Data geomview redraw Camera This ensures that camera updates are not done during the update avoiding unnecessary display updates which would slow down the visualization Note that this example does not implement the framerate feature and would be extremely slow due to the very high number of Geomview updates at every time step 4 The Spring Loaded Inverted Pendulum An Example Dynamical System 4 1 The Continuous Mathematical Model The SLIP model considered in this example is shown in Figure 4 The leg is assumed massless and the body a point mass at the hip joint During stance the leg is free to rotate around its toe and the mass is acted upon by a radial spring with potential U q In flight the mass 22 is considered as a projectile acted upon by gravity We assume there are no losses in either the stance or flight phases Under these assumptions the stance dynamics are given by Dy Uk qr sin qor m Day Uk qr COS Gor m where k is the spring constant and m is the body mass 4 2 The Hybrid Locomotion Model During steady state locomotion our model goes through four consecutive charts ascent descent compression and decompression Among these ascent and descen
23. account more elaborate components such as the battery voltage drop as a function of current and discharge time or the effects of ambient temperature Similar to the electrical motor model the battery model is implemented outside the SimSect simulation environment 5 1 8 Relevant Coordinate Transformations e Positional leg states in the body frame Vi Ven Vys Ves Re fi rp ai arctan a y 22 vi 6 0 arctan 2 y 2 Vrt yet 2 e Leg velocities in the body frame ves ty S i ip R Vi ai ou D Ava VCF v l Qi pil Uz y Uyt C E VF 41 e Leg accelerations in the body frame a 3 i T _1 i eS E Sit ie Vi Ries Uy s z Rb i rbp gt Rp Vi ai 2K Vi Ypo S l Qi pi 22 Avz D H C v3 3B ve 2BCvx2 C be Cve 05 02 Huy y 02 t2 C3 2 C u3 2B vz y Uy z T C Vz y A Uy z C 4E AF we i2 F Ur r Vy y SA Uz z 4F where Uz n Un z Vy Vy Uz Uzi 2 2 Uy Uz 2 3 Vy Vaz Ux Vy Vy Ug Vay F Vy Vy T Uzi Vz 2 2 2 Up F Uy F Uz ae oaQqgu s I and 5 Fr rbp Wp M J wb Mwp Tr Ry Rh J wp Rp I wp Rp Ben E E a e Toe coordinates in the world frame Pi sin 0 pi cos 0 sin Q pi cos 8 cos Qi fi Rs Vi ai r Vi 5 2 The Open Loop Control of Locomotion In
24. at time instant Note that this function also takes care of defining and handling the measurement stopping function introduced in Section 2 4 2 as well as the recording of trajectory points through the use of the flow memory see Section 3 4 1 3 3 4 The Stopping Iterator This module is responsible from refining the last time step of the integration in a chart to determine the exact boundary crossing time Figure 3 depicts the algorithm for the iteration function of this module This iterator is invoked from within the wrapup function of the flow iterator and uses a midpoint subdivision type algorithm to refine the time step where the boundary crossing is detected It can handle multiple crossings and returns the crossing which is the earliest in time The functions associated with this module are also defined in the files SS_ComputeFlow c and SS_ComputeFlow h The stopping iterator is somewhat hidden in the flow computation module so it does not have any external functions which provide access to its iteration functions The exported function are mainly for defining and deleting stopping functions and are heavily used by both the integrator engine itself for defining final time stopping functions measurement stopping functions etc and by the model definition to define and specify boundary functions 10 e SS_AddStopFunc Adds a stopping function to the current list of active stopping functions These are scalar valued functions whose
25. ata to ensure consistency Note that this function first compares the current time to the last time it has been called and therefore does not carry out the same computation more than once 49 This synchronization function uses many of the other support functions for its function ality Below is a list of those functions summarizing their functionality Moreover the order that they are listed in reflects the order that they should be called in to ensure that the value returned is up to date and consistent The reason for this requirement is that most of these functions use each other to compute certain things and to avoid redundant computations imposing an order is necessary e hex_BodyPos Returns the position of the rigid body in W e hex_BodyVel Returns the velocity of the rigid body in W e hex_BodyR Return the rotation matrix for the rigid body e hex_Bodyw Returns the angular velocity of the rigid body in W e hex_BodyRd Returns the derivative of the body rotation matrix e hex_FootPos Returns the position of a particular toe in W e hex_FootVel Returns the velocity of a particular toe in W e hex_LegStateCart Returns the cartesian vector state V for a particular leg e hex_LegVelCart Returns the cartesian vector velocity V for a particular leg e hex_LegAngles Returns the leg state in polar coordinates e hex_LegVel Returns the leg velocity in po
26. ations for visualization of the system trajectory using Geomview The integration algorthms invoke these functions to compute necessary components of the computation 3 3 The Integrator Engine The overall structure of SimSect is depicted in Figure 2 where the example hexapod model components is also included This section describes the the integrator engine and its in terface to the model definition The subsections summarize C implementations of various components The reader should refer to the actual source code for implementation details 3 3 1 Iterators Most numerical algorithms use some form of iteration where a particular procedure is repet itively executed until a certain condition is satisfied Several components of the numerical integration procedures in SimSect have this structure On several levels SimSect makes use of an abstract iterator concept An iterator is a computational object which consists of an initialization procedure an iterating function a termination condition and a wrap up procedure When an iterator is invoked the ini tialization is followed by repeated execution of the iterating function until the termination condition is met The wrap up procedure is then invoked and terminates the iteration 7 Model Definition Trajectory Integrator SimSect rc SS_LoadInitFile oe oa SS_SymbolTable SS_InitSystem SS_InitSymbolTable SS_GetSymbol SS_ComputeTraj SS_PutSymbol hex_DefineChart flow_initia
27. ce into initialization handling of the frame rate and the computation of the transforms is very typical int slip_Geomview double x double p int chart double f 48 int num static double lastTime 0 static int frameNo 0 if x TIME 0 lastTime 0 frameNo 0 if x TIME gt lastTime SS_GeomviewSend amp SS_Data geomview freeze Camera n num slip_Transform f x p chart 33 SS_GeomviewTransform amp SS_Data geomview num f SS_GeomviewSend amp SS_Data geomview redraw Camera n frameNot lastTime 1 0 FRAMESPERSEC return NO_ERROR The following function computes the transformation matrices for the SLIP geometry and the camera to be sent to Geomview In the exampel SLIP model the hopper is defined as a single object consisting of a sphere and a vector for the leg The first transformation matrix rotates and positions this object according to the current system state The second transormation matrix sets up the camera such that it follows the hopper from a convenient angle to provide a nice visual animation of the simulation int slip_Transform double f double x double p int chart double angle double z_angle 0 9 PI 2 x_angle 1 8 PI 4 double camera_x memset f 0 2 16 sizeof double This section transforms the body object to its current position and orients the leg based on what the leg angle is Note that during stance the f
28. code is included for illustration purposes and should clarify many of the concepts involved in defining a hybrid model in SimSect 23 4 3 1 The State Space The state space for the SimSLIP implementation is defined as follows xXx I z Y T Y fis fy qot The SimSLIP implementation defines the following state names for initialization file pur poses Xnames x y xdot ydot footx footx touchdown_angle Note that the states fr fy and qos are bookkeeping states and are only updated at chart transitions 4 3 2 The Model Parameters The SLIP model makes use of several parameters configurable from within the initialization file The parameters and their associated names are defined as follows P m rts Urls Ist Ifl FPS qro ty J k 1 Pnames body_mass q rt q r1l g stance g flight framerate q 0 spri sprj k Note that 2 j and qro parametrize the spring law in the following form Uiii dr dro k P x zp l EN 4 3 3 Some Macro Definitions The following macro definitions are used in all the function definitions and make the model code much more readable Parameters define M_BODY p 0 define Q_RT p 1 define Q_RL p 2 define GSTANCE p 3 define GFLIGHT p 4 define FRAMESPERSEC p 5 define SPR_REST p 6 define SPR_I pL7 define SPR_J p 8 define SPR_K p 9 24 define NUM_OF_PARAMS 10 Names of charts define ASCENT_CHART 0 define DESCENT_CHART 1 defin
29. compliant hexapod model we chose to have linear springs and viscous dampers for both degrees of freedom Moreover the radial spring saturates with an exponential force past a certain length modeling the limited extension range of actual leg implementations Fp i kr pi Po dr Bi Pi lt Pi 2 i K exp pi po kr Pi Po dr Pi otherwise Tho 3 ko 0 bo do 6 5 1 3 Total Force and Torque on the Body The cumulative effect of all the legs on the body is simply the sum of the individual contri butions together with the gravitational force Expressed in W the force and torque vectors are given by 38 0 6 Fr 0 R leg F 3 Mag i l 6 Tr Rp So legit 4 i l where we define ee a 0 leg iis in flight oS 1 leg i ts in stance 5 1 4 Rigid Body Dynamics The dynamics of a rigid body under external force and torque actuation is governed by the following equations 2 E Fr rb Mp Mwy J wp Mwy TP Ry J wp Rp where we have 0 w aby T J wa Wy Wz fh We 0 wy Wy Wy 0 M RpMoR 5 1 5 Feet Dynamics If a leg is in flight then its toe experiences the negative of the force computed in Section 5 1 2 for that particular leg Hence the associated vector field takes the following form a ae If on the other hand a leg is in stance then the vector field incorporates a first order friction model to model the motion of the toe ane
30. con troller parameters te and These parameters are enforced by SS_OpenLoopVectorField which computes the derivatives of the reference trajectories The vector field for each of the reference trajectory states is given below Two functions openLoopBoundary and halfCycleBoundary implement the four bound ary functions for each leg While openLoopBoundary detects the transition from the slow to fast leg swing halfCycleBoundary detects the middle of slow and fast phases The tran sitions initiated by openLoopBoundary only update the controller vector field whereas the transitions of halfCycleBoundary are also responsible from changing the controller param eters te ts s and Qo See the function hex_OpenLoopTransition for details 5 5 4 The PD controller The function hex_OpenLoopControl implements six simple PD controllers around the refer ence trajectory encoded by the states associated with the controller This is where the hip 52 torques for each leg are computed as a function of the current leg position and the desired reference position 5 6 Future Work on the Hexapod Model The current hexapod model has many flaws The first and the most problematic one is the chattering between different leg modes Chattering in hybrid dynamical systems occurs when the vector fields of two neighboring charts point toward their common boundary and the trajectory has to slide along the boundary The SimSect integrator currenty cannot handle
31. ding the init file the parameter symbol names the number of charts and the number of 17 auxiliary functions The initial state and the default parameters should be pointers to arrays of doubles which are then copied by this function The state and parameter names must be pointers to arrays of strings with the corresponding symbols e Perform any setup or initialization needed by the model The initialization file is pro cessed before SS_InitSystem is called Hence the system parameters initial conditions etc can be accessed by this function to perform the initialization 3 5 3 Initializing the Partition The function pointer in SS_Data ipFunc after the model initialization points to this function The main purpose of this function is to finalize the initial conditions and determine which chart the system will start from in addition to any modifications to the parameters and or model states It is called after all the system initializations are complete and right before the integration starts The prototype for this function is int InitPartition double xnew int newChart double x double p int chart The fields indicated by xnew and newChart must be filled out by the function and they will be the initial state and the initial chart for the integration to come 3 5 4 The Vector Field This is the function pointed to by the variable SS_Data vfFunc Its main purpose is to compute the vector field for the model at a given sta
32. e Opens the specified text file and sends the contents to Geomview This function is most commonly used by the model in defining the initial geometry of the visualization by loading a static oog1 file during initialization e SS_GeomviewClose Closes the connection to Geomview e SS_GeomviewClear Deletes all the objects in Geomview 3 4 6 Miscellaneous Utility Functions The files SS_Util c and SS_Util h provides several utility functions to the model program mer Among those functions are general purpose matrix and vector manipulation procedures special purpose matrix and vector facilities for manipulating vectors in three dimensions and rotation matrices as well as some other functional tools Please refer to the source code for details on these utilities and their usage 3 5 The Model Definition Programming a model in SimSect involves providing several functions defining different com ponents of the hybrid system formalism as well as functions to interface to the visualization subsystem The following list summarizes functions that the programmer must provide in order to complete the definition of the model The following subsection describes each of these functions in detail including their inputs outputs and the requirements on their func tionality e Setup model structures define state and parameter symbol names e Initialize the partition structure the initial state and the initial chart e Compute the vector
33. e COMPRESSION_CHART 2 define DECOMPRESSION_CHART 3 4 3 4 Model Initialization The first task of the model initialization function is to define the default initial state and default parameters for the model as well as their names This is then followed by informing the integrator which functions the model definition provides and a call to SS_SetupSystem to finalize the initialization int slip_InitSystem void static const double state 0 0 0 9 1 0 0 0 0 0 0 0 0 0 static const char stateNames x y xdot ydot footx footy touchdown_angle F static const double params 50 48 1 0 1 0 10 10 120 1 0 1 0 2 0 1000 F3 static const char paramNames body_mass q_rt q_rl g_stance g_flight framerate q_0 spri sprj k F static int numStates NUM_OF_STATES static int numParams NUM_OF_PARAMS static int numPartitions 4 static int numAuxFunc 6 SS_Data vfFunc slip_VectorField SS_Data trFunc slip_Transition SS_Data vcFunc slip_ValidateChart SS_Data dcFunc slip_DefineChart SS_Data ipFunc slip_InitPartition SS_Data geomFunc slip_Geomview SS_Data auxFunc slip_AuxFunc return SS_SetupSystem numStates state stateNames numParams params paramNames numPartitions numAuxFunc 25 4 3 5 Initializing the Partition The initialization of the partition in the SLIP model involves determining which chart the specified initial confitions c
34. e required functions for the visualization inter face The main two functions are hex_GeomviewInit and hex_Transform The first function handles the initialization of the scene by loading the hexaped oogl1 file into Geomview and by creating and defining the terrain object using the terrain_height function The second function computes the transformation matrices for all the objects in the scene that need to be relocated Normally those are the rigid body the legs and the feet Consequently this function computes 13 transformation matrices as functions of the system state These matrices are then sent to Geomview by the wrapper function hex_Geomview function resulting in the update of the visualized scene Note that the wrapper function also implements a frame rate feature to avoid excessive update of the display which would unacceptably slow the integration 5 4 8 Support Functions The files SS_HexForces and SS_HexTransform implements several functions to support the implementations of the functions described above One of the most important support functions is hex_syncStates It computes several values used in the vector field as well as the controller Starting from the main state variables x it fills in all the required fields in SS_HexapedData in the correct order making sure that at the end the struct contains consistent values for the current data point Hence this function is to be called first by any function that use the associated d
35. e tolerance determine the accuracy of the integration algorithm e minTimeStep default 1e 15 The minimum allowable time step for integration The time step used by the integra tion algorithm is not allowed to go below this value e power default 0 33 The time step adjustment power for the Runge Kutta algorithm 3 Programming with SimSect 3 1 Hybrid Dynamical Systems A large number of dynamical systems that we are interested in analyzing including the hexapod model that we present in this report cannot be represented with a single dynamical flow They are hybrid dynamical systems which are mixtures of discrete and continuously varying events This section describes a formal definition of hybrid dynamical systems and is mostly quoted from the formalism described in 1 with some minor modifications We assume that the problem domain is decomposed into the form mole acl where J is a finite index set and Va is an open connected subset of R Each element in this union is called a chart Each chart has associated with it a vector field fa Vax R R Notice that the charts are not required to be disjoint Moreover on the intersection set Va Vg continuity or even agreement of the vector fields are not required for a I For each a I the chart V must enclose a patch an open subset U satisfying Ua C Vo The boundary of U is assumed piecewise smooth and is referred to as the patch boundary Together
36. ection 5 3 with a specification of the chart transitions Section 5 1 1 describes the derivation of the physical model and assumptions followed by Section 5 1 2 and 5 1 3 where the force and torque vectors acting on the hexapod body are computed Finally Section 5 1 4 gives the equations of motion for the hexapod followed by Section 5 1 8 where several coordinate transformations involved in the computations are given 35 Figure 5 The compliant hexapod model 5 1 1 The System Structure Figure 5 shows the structure of the hexapod model The system consists of a rigid body with six degrees of freedom whose position and orientation are described by rp and Rp respectively Two coordinate frames B and W are defined the former attached to the hexapod body and the latter an inertial frame where the dynamics are formulated The legs are attached to the rigid body at fixed points a in the body frame Each leg has complete spherical freedom Each leg ha a very small mass m at the toe useful in modeling the flight behavior of the legs as well as a simple friction model during stance Note that rp Rp vi and f are related through a simple coordinate transformation see Section 5 1 8 Associated with each leg there is a radial and a torsional spring on the 0 direction as well as torque control on the degree of freedom These springs and the hip actuation result in forces and torques being applied to the rigid body In Section 5 1
37. ed Two new gain param eters At and Ad are introduced Consequently turning in 2 right direction is achieved by using u te ts Ats bs Po Ado and u te ts Ats bs do Ago for the legs on the left and right sides respectively 5 3 The Hybrid Hexapod Model 5 3 1 The State Space The state space of the hybrid hexapod model is more than just the state of the rigid body The states of each leg are also needed in order to be able to compute the vector field of the previous section Consequently the following lumped vector defines the state space for the hybrid model x I Rb Wp rb fp fi fi 5 3 2 The Partition Structure The mathematical model of the previous section does not address any of the discrete tran sitions that take place during locomotion Discrete events in the model come from the collisions of the feet with the ground changing the states of the flags in 3 Each leg can be in one of two modes stance or flight Hence the compliant hexapod model has a total of 2 64 discrete charts capturing all the possible structural configu rations of the hexapod Note that this structure does not take into account the possible collision of neither the rigid body nor the legs themselves with the ground It is assumed that collisions occur only between the toes and the ground surface In each of these 64 charts the model obeys the continuous dynamics of the previous section with the appropriate leg flags le
38. ed on the upcoming parameter set and phase of the leg The function hex_OpenLoopVectorField implements the controller vector field 5 5 2 Controller Partition Structure Throughout the open loop reference trajectory generation each leg goes through four phases the two halves of the slow phase and the two halves of the fast phase Zo 0 ts 2 T ts 2 te 2 T2 t 2 ts 2 T t 2 0 See Figure 8 Consequently there are 4 different possible combinations of leg phases yielding 4096 different controller charts The definition of the controller partition assigns two bits to each leg in the chart value of the simulation Bits 17 6 encode the phases for all 6 legs while bits 5 0 encode the current touchdown states for the legs Note that the transition associated with these chart components are purely time depen dent This underlines the open loop nature of the controller where the hexapod state does not affect the controller state in any way except the PD controller which implements local feedback at each leg 5 5 3 Controller Transitions The transition function associated with the controller is where the vector field of the corre sponding leg is updated Moreover changes in controller parameter values take place during transitions from leg phase 3 0 and leg phase 1 j2 where all the legs are assumed to undergo the transition at the same time The SimSect implementation of the hexapod model incorporates only two of the
39. ells eomviewSend amp SS_Data geomview read geometry define platform_geom CMESH ntf str i i n int GRID_XSTEPS 3 int GRID_YSTEPS 3 eomviewSend amp SS_Data geomview str county 0 county lt GRID_YSTEPS 3 countyt r countx 0 countx lt GRID_XSTEPS 3 countxt color_flag countx county 2 1 height 0 0 if countx int GRID_XSTEPS 2 0 ptx 0 5 countx 1 0 GRID_XSTEPS 2 height 0 1 else ptx 0 5 countx 1 1 0 GRID_XSTEPS 32 at every trajectory data point if county int GRID_YSTEPS 2 0 pty 1 0 county 25 0 GRID_YSTEPS 2 height 0 1 else pty 1 0 county 1 25 0 GRID_YSTEPS sprintf str Wf AE Ya AE AE AE Yen ptx pty height color_flag 0 333 0 2352 color_flag 0 4196 0 7019 color_flag 0 1843 0 443 Tzs SS_GeomviewSend amp SS_Data geomview str SS_GeomviewSend amp SS_Data geomview SS_GeomviewSend amp SS_Data geomview redraw Camera n The next component of the interface is the function which gets called by the integrator The main purpose of this function is to implement the appropriate frame rate by periodically sending updates of the transformation matrices to Geomview Note that the actual computation of the transformation matrcies is done in slip_Transform which is also detailed below This kind of decomposition of the interfa
40. ent is necessary to be able to define stopping functions which are local to the particular flow iterator they are defined in See Section 3 5 6 for details on how to define and add stopping functions for a particular chart 3 5 6 The Boundary Functions Unlike the other functions provided by the model the definition of the boundary functions is somewhat transparent to the integrator and is not reported through any of the variables in the global struct SS_Data Instead the stopping function definitions include the pointers to the corresponding boundary functions SS_Stopping h defines the C type for stopping function definitions as typedef struct int index Indices for the boundary function int type Crossing for the boundary function int state The current state of each function double value double timeStep int action double val double x double pL int chart int ind SS_StopFunc When defining boundary functions in for example the DefineChart function the model programmer needs to fill in the index type and action fields of this struct prior to calling SS_AddStopFunc The type field is either STOP_TYPE_LEFT or STOP_TYPE_RIGHT specifying whether the boundary function crossing should be detected from the left or right resulting a time step either before or after the crossing respectively this is necessary because the 19 crossing is detected only up to a certain precision The index and acti
41. exapod useGeomview 1 framerate 30 Integration engine related parameters finalTime 5 maxChartCount 128000 stopPrecision le 10 maxsStoplterations 128 tolerance le 5 maxTimeStep 1e 3 minTimeStep 1e 15 rkPower 0 33 Figure 1 An example initialization file for SimSect dataBaseName default SimSect The base filename for the output files The suffixes data initial and param are appended to this to obtain the output filenames useGeomview default 0 Flag to enable the link to Geomview for the visualization of the simulation The model definition must provide a visualization mapping function for this option to work properly stopPrecision default 1e 10 Numerical precision tolerance for the stopping functions The boundary crossings between different charts in the hybrid system are detected up to this precision in the function value maxStopIter default 128 Maximum number of iterations that the stopping iterator will be allowed to go through before giving up on trying to find the boundary crossing The failure to find a cross ing within this limit strongly suggests that there is a discontinuity in the boundary function recordPeriod default 0 0 The time period for the recording of trajectory data which will be saved in the output files Special stopping functions are defined to compute the trajectory points at exact multiples of this value resulting in accurate
42. files SS_Tokenizer c SS_Tokenizer h SS_SimScript c SS_SimScript h and SS_InitFile c The exported procedures and their functionality are as follows e SS_InitTokenizer Creates and initializes a tokenizer object e SS_OpenTokenizer Starts the tokenizer with a particular file or string input This is the first step to be taken before any module can use the tokenizer e SS_GetNextToken Identifies and retrieves the next token from the input stream e SS_CloseTokenizer Ends the tokenization from the current stream and closes the stream e SS_EndOfInput Checks whether the input stream has reached its end e SS _RunScript Opens a specified stream and interprets the script read from the stream Here the script format is the initialization file format described in 2 4 1 e SS_LoadInitFile Loads and interprets the initialization file with the filename supplied as an argument This is the function used by the top level SimSect execution It is simply a wrap per to SimScript facilities which in turn use the parser and the tokenizer for their functionality 14 3 4 4 The Root Finder Utility Module Some of the controllers used in the example models require the computation of the roots of a scalar function Consequently SimSect now provides a Newton s method root finder as a utility module This module also uses the iterator facilities of SimSect as it relies on an iterative algorithm Please
43. g 5 3 3 Chart Transitions When the rigid body and the legs are moving each of the legs can transition from stance to flight and vice versa independently Consequently it is possible to transition from each of the 64 charts defined in the previous section to the remaining 63 In this section we de scribe a structured way of defining these transition conditions together with their associated boundary functions In a particular chart we define transition conditions associated with each leg depending on their current mode There are two types of boundary functions liftoff and touchdown associated with each leg which is either in stance or flight respectively e Liftoff condition 44 The liftoff boundary function is the component of the ground reaction force acting on the toe normal to the ground surface When this force becomes positive the leg is forced to lift off This function takes the following form bi x n4 fois fui F This condition alone however is not adequate to ensure proper definition of the liftoff model The exponential component in the radial spring definition of 2 is critical in proper functioning of the liftoff condition e Touchdown condition During flight a leg touches down when its toe reaches the ground The boundary function associated with this condition takes the following form B x 0 0 1 J71 Pal Foss fu Upon detection of this boundary crossing the ve
44. g over and the body hitting the ground In any one of the charts if y lt 0 at any point on the trajectory the state chart combination becomes invalid The following implementation of the chart validation function ensures proper detection of this failure mode int slip_ValidateChart int in_chart double x double p int chart in_chart SS_TRUE if x 1 lt 0 The body is underground SS_Message Error Body toppled over n in_chart SS_FALSE 31 retu 4 3 11 rn NO_ERROR The Geomview Interface The first component of the Geomview interface is the initialization function This function loads the geometry file slip oogl into Geomview and then defines a 5x100 grid platform that the runner will be running on The details of the platform construction are not important However the interested reader might refer to 3 for details of how to create and manipulate geometries in Geomview define GRID_XSTEPS 5 define GRID_YSTEPS 100 void slip_GeomviewInit double p int int char doub SS_G SS_G SS_G SS_G D SS_G spri SS_G for fo countx county color_flag str 2048 le ptx pty height eomviewClear amp SS_Data geomview eomviewSendFile amp SS_Data geomview slip oogl eomviewSend amp SS_Data geomview backcolor Camera 0 25 0 21 0 5 eomviewSend amp SS_Data geomview freeze Camera n efine the platform with the specified number of grid c
45. irely in Ua x a for all t ti tis e Fort t ti 1 and the unique a specified above t gt m 7 t is an integral curve of the vector field fa e lim m y t y exists y E Sa and Taly lim q t i i 1 tots 3 2 System Architecture SimSect isolates the integration algorithms from the definition of the dynamical system Unlike the model implementation which is different for every system model the integration engine is a static component of the system This abstraction makes the implementation of additional model definitions and the modification of existing models much simpler Defining a dynamical system model consists of providing the hybrid components described in Section 3 1 The model definition is usually programmed by the user and includes functions corresponding to the following list of tasks e Initialize the partition structure the initial state and the initial chart e Define the properties of a particular chart and the boundary functions that will be used e Compute the vector field for the individual charts e Compute boundary functions identified by a certain index that the chart initialization determines for the current chart e Perform chart transitions by computing the next system state and chart e Validate a chart by checking whether a given trajectory point lies in the chart e Compute an auxiliary function mainly used for data collection purposes e Compute the homogeneous transform
46. lar coordinates e hex_ComputeLegForceTorque Compute and return the forces and torques produced by a particular leg e hex_FootAccel Compute and return the acceleration of a toe in W 50 e hex_ComputeForceTorque Compute and return the total force and torque on the rigid body in W e hex_BodyAccel Returns the acceleration of the rigid body in W e hex_Bodywd Computes and returns the derivative of the body angular velocity vector in W e hex_BodyRdd Computes and returns the second derivative of the body rotation matrix e hex_LegAccelCart Computes and returns the second derivative of the cartesian leg state vector Vi e hex_LegAccel Computes and returns the second derivative of the polar leg state vector v Note that the functions specific to individual legs must be called for all the legs before proceeding with the hex_ComputeForceTorque function Please refer to the source code for the particulars of the SimSect implementation 5 4 9 Defining Controllers In the hexapod model the controllers also have their own hybrid structures with vector fields and transition functions The file SS_HexControl c implements the interface to possible different controllers supported by the model The configuration parameter control_type selects the active controller for the simulation The functions in SS_Control c route the hybrid simulation system calls to the appropri ate controller
47. lize flow_iterate flow_wrapup gt hex_Transition SS_AddStopFunc SS_GetStopFunc stop_initialize stop_iterate stop_wrapup SS_ComputeFlow PIR integ_iterate integ_wrapup hex_OpenLoopreference hex_AuxFunc SS_FlowMem hex_Geomview hex_Transform SS_Geomview SS_FlowMemAdd hex_GetHipTorque SS_GeomviewSend SS_GeomviewTransform SS_GeomviewSendFile hex_GetBendTorque hexaped oog1 SS_RungeKutta SS_ClearStopFunc SS_Stopping hex_Boundary i hex_GetradialForce hex_VectorField RK_initialize RK_iterate RK_wrapup Figure 2 System architecture of SimSect with an example model instance of the Compliant Hexapod Model The files SS_Iterator c and SS_Iterator h define functions to initialize and use itera tors at a somewhat abstract level The functions defined by this subsystem are summarized below e SS_InitIterator Setup an iterator by defining the appropriate initialization iteration interruption and wrapup procedures e SS_Iterate Invoke an iterator by first calling its initialization procedure then repeatedly calling its iteration procedure until the termination flag is set and then calling its wrapup procedure SimSect implements a hierarchy of several iterators implementing different components of the integration The following sections describe various iterators in SimSect 8 3 3 2 The Runge Kutta Iterator This iterator implements the 4th order adaptive time step Runge
48. model which will then reflect themselves back to the vector field in the fact that a new chart has been entered 18 3 5 5 Defining a Chart The variable SS_Data dcFunc points to this function after initialization The main purpose of this function is to define a chart after each chart transition which occurs after boundary crossings The definition of the chart almost always means defining the stopping functions associated with the new chart based on the new boundary functions which become active fol lowing the transition This function will use the stopping function facilities of the integrator to create and define the stopping functions Note that the places in which the stopping function are accessed transition function etc are separate from this function Consequently the indices returned by the stopping function definition utilities must either be stored in some place accessible from those other procedures or be based on a common indexing mechanism which can later be used to recover the indices for the stopping functions These indices are assigned to stopping function in the order they are defined starting from 1 Hence if the order of their definition is carefully chosen it will be possible to know in other parts of the model what these indices will correspond to Example systems of later sections will make this clearer The prototype for this function is int DefineChart SS_ComputeFlow cf int chart Note that the cf argum
49. n break case COMPRESSION_CHART newChart DECOMPRESSION_CHART SS_Message Transition COMPRESSION gt DECOMPRESSION n break case DECOMPRESSION_CHART double legx x 0 x 4 double legy x 1 x 5 Make the trivial touchdown decision xNew 6 atan2 legx legy newChart ASCENT_CHART SS_Message Transition DECOMPRESSION gt ASCENT n break default SS_FatalError slip_Transition INVALID_PARAM_ERROR return INVALID_PARAM_ERROR trans SS_TRUE 30 else trans SS_FALSE return NO_ERROR Note that the state chart combination which evaluate to negative boundary function should also be considered invalid states In our simple illustrative example however we do not explicitly check for these cases in the chart validation 4 3 9 The Auxiliary Functions The data to be recorded is computed by the auxiliary functions For our SLIP example we record the time together with the position and velocity of the body Note that the number of auxiliary functions is defined in the model initialization function and must be consistent with the number of variables filled in by the auxiliary function component int slip_AuxFunc double f double x double p int chart f 0 x TIME f 1 x 0 f 2 x 1 3 x 2 f 4 x 3 return NO_ERROR 4 3 10 Chart Validation The simple SLIP model has only one potential mode of failure The hopper topplin
50. n any memory these states become critical in keeping track of the discrete changes that take place in the system Exam ples of such states are counts of chart transitions the foot placement location for the SLIP model mechanisms to keep track of currently active tripods in the hexapod model etc 3 5 2 Initializing the Model This is the first component of any model called by SimSect This function is called from within SS_InitSystem which looks at the parameter systemName to figure out which model is to be integrated Hence when a new model is to be defined the programmer needs to modify SS_InitSystem to call the model initialization function This static way of specifying the model initialization functions is because SimSect currently does not have any means of dynamically loading precompiled model definitions during execution Hence their entry points must be hard coded in SimSect The model initialization function needs to carry out the tasks in the list below e Fill in the model function fields in the global SS_Data struct The fields that need to be filled out are SS_Data ipFunc SS_Data vfFunc SS_Data dcFunc SS_Data trFunc SS_Data vcFunc SS_Data auxFunc and SS_Data geomFunc each of which correspond ing to one of the model functions described in the following sections e Call SS_SetupSystem specifying the number of states the initial state the names of state variables the number of parameters the default parameters before loa
51. n environment http www geom umn edu to visualize the state of the dynamical system in a flexible manner If you have questions or comments about SimSect or this manual you can e mail them to ulucs eecs umich edu 1 2 Authors SimSect is originally written by Uluc Saranli together with the Spring Loaded Inverted Pendulum SLIP and the Compliant Hexapod Model definitions 1 3 Overview SimSect is designed to be used for integrating a well defined hybrid dynamical flow In doing so the user can specify several parameters of the dynamical system as well as the initial conditions without having to modify the source or recompile the program The output of SimSect is a set of user defined functions evaluated over the entire computed trajectory SimSect can either be used as a batch simulation tool or as a real time visualization environment for the dynamical system The former case can proceed without any user interaction where the initialization files are usually created through a scripting language such as Perl and the results are obtained in a set of output files In the latter scenario however the user modifies the initialization file runs the simulation and observes the results through the visualization The definition of a model in SimSect involves writing C code corresponding to various components of the hybrid dynamical system These include the definition of the vector field on different discrete charts of the system the chart
52. ocity of the toe is set to zero The model then checks the normal omponent of the ground reaction force The chart transition does not occur if this force is positive to avoid an invalid chart state pair The main reason why such an exception is handled by the model is the damping in the leg Due to the plastic collision and the toe velocity being set to zero there is a step change in the derivatives of the spherical leg states at every touchdown This in turn results in discontinuities in the ground reaction force as a result of damping in the leg yielding this exception Note that because the normal toe force is positive the toe starts traveling away from the surface validating the skipping of the touchdown transition 5 4 Implementation of the Model 5 4 1 Configuration Parameters Table 2 summarizes the configuration parameter symbol names for the compliant hexapod model together with their default values 45 Table 2 Compliant hexapod model configuration parameters Defaulk Hexapod platform parameters body_mass Body mass kg leg_mass Toe mass kg friction_coeff 0 5 Surface friction coefficient q rt Touchdown leg length m 1 Liftoff leg length m 10 Gravitational acceleration m s max_torque Maximum leg torque output Nm 10000 max_speed 10000 No load motor rotation speed rpm speed_constant 61 261056 Ky rad Vs j Or re k jai torque constant K Nm A armature_resist Tal
53. on fields define what boundary function should be used to evaluate the stopping function The index field is mainly provided as a convenience for models where it makes most sense to parameterize certain boundary functions with an index corresponding for example to the current chart Consequently whatever function is assigned to the action field of any stopping function specification must define the corresponding boundary function for all possible indices that they might be called with As it can be seen from the prototype the boundary functions take the current state model parameters and the current chart and return the value of the boundary function corresponding to the specified index In any of the model functions the programmer might retrieve one of the boundary functions through SS_GetStopFunc and check the function value in the value field of the above struct This is for example the way in which the transition function determines which boundary function caused the transition and decide on the appropriate course of action 3 5 7 The Transition Function The transition function is the function pointed to by the variable SS_Data trFunc after model initialization The main purpose of this function is to determine the new state and chart after a boundary crossing Any zero of any stopping function encountered during integration stops the flow iteration and calls the transition function to determine what the appropriate course of action is I
54. oot position relative to the body position determines the leg angle However during flight we assume that the leg immediately goes to its touchdown position because it has no mass if chart COMPRESSION_CHART chart DECOMPRESSION_CHART angle atan2 x 0 x 4 x 1 f 0 1 0 f 5 f 10 cos angle f 9 f 6 sin angle camera_x f 13 x 0 f 14 x i f 15 1 0 else f 0 1 0 f 5 10 cos x 6 34 f 9 f 6 sin x 6 camera_x f 13 x 0 f 14 x 1 f 15 1 0 This transformation matrix follows the hopper from a nice angle f 16 0 cos z_angle f 16 1 sin z_angle f 16 2 0 f 16 4 cos x_angle sin z_angle 16 5 cos x_angle cos z_angle f 16 6 sin x_angle 16 8 sin x_angle sin z_angle f 16 9 sin x_angle cos z_angle 16 10 cos x_angle f 16 12 3 8 f 16 13 camera_x f 16 14 1 4 f 16 15 1 0 return 2 5 The Compliant Hexapod A More Complicated Ex ample This section describes the details of the Compliant Hexapod Model definition in SimSect The hexapod model originally motivated the SimSect project and is one of the example model definitions 5 1 The Continuous Mathematical Model In this section we derive the continous dynamics of the hexapod within a particular chart The definition of the hybrid model is completed in S
55. orrespond to and then initialize the Geomview component The model assumes that the initial leg length is q Together with the initial leg angle and body position this determines whether the tip of the leg is touching the ground or not The velocity of the body in turn determines which chart in flight or stance the system is initially in int slip_InitPartition double xnew int new_chart double x double pL int chart double tipy x 1 Q_RT cos x 6 memcpy xnew x NUM_OF_STATES sizeof double if tipy lt 0 Start at stance double tipx x 0 x 1 tan x 6 double qrdot x 0 x 2 x 1 x 3 sqrt x 0 x 0 x 1 x 1 xnew 4 tipx xnew 5 0 0 if qrdot lt 0 This is compression new_chart COMPRESSION_CHART SS_Message Starting in COMPRESSION n else new_chart DECOMPRESSION_CHART SS_Message Starting in DECOMPRESSION n else Start at flight if x 3 gt 0 This is ascent new_chart ASCENT_CHART SS_Message Starting in ASCENT n else new_chart DESCENT_CHART SS_Message Starting in DESCENT n slip_GeomviewInit p return NO_ERROR 26 4 3 6 The Vector Field The flight charts and the stance charts have different vector field definitions During as cent and descent the body is acted upon by gravity only During compression and de compression it also feels the spring force in addition to gravity
56. puteTraj c and SS_ComputeTraj h e SS_InitTraj Initializes necessary structures for the trajectory iterator e SS_PurgeTraj Purges memory structures allocated by the InitTraj function for a trajectory iterator e SS_IntegTraj Integrates the dynamical system from an initial condition until either the final time or the maximum chart count is reached The integration also stops in case of an error The integration is accomplished by invoking the flow iterator for each chart calling appropriate transition functions for boundary crossings This is the main entry point to the integrator and is called right after the initialization carrying out the integration Following the invocation of this function the trajectory data is saved and the system exits see Section 3 3 6 for details 11 3 3 6 The Top Level This section describes what happens at the topmost level of SimSect from within which all the other modules are invoked The file SimSect c is the entry point to the system The main sequence of tasks carried out by the system from startup to the end are as follows e Initialize the symbol table and load the initialization file e Initialize Geomview subsystem e Call SS_InitSystem This function chooses the model to be integrated and calls its initialization function Note that to add a new model this function should be modified to incorporate the new model s initialization system e Initialize the flow memory
57. re in contact with the ground is determined by the duty factors of both tripods Finally the parameter offsets the motion profile with respect to the vertical see Figure 8 Note that both profiles are monotonically increasing in time but they can be negated to obtain backward running Control of running behavior is achieved by modifying these parameters for a particular desired behavior during locomotion 5 2 2 Turning We have developed two different controllers for two qualitatively different turning modes turning in place and differential turning during running The controller for turning in place employs the same leg profiles as for running except that contralateral sets of legs rotate in opposite directions This results in the hexapod turning in place in the direction determined by the rotational polarity of the left and right sets of legs Note that the tripods are still synchronized internally maintaining three supporting legs on the ground Similar to the control of the forward locomotion speed the rate of turning 43 depends on the choice of the particular motion parameters mainly te and 3 In contrast we achieve turning during forward locomotion by introducing differential perturbations to the forward running controller parameters for contralateral legs In this scheme te is still constrained to be identical for all legs which admits differentials in the remaining profile parameters and t while remains unchang
58. s are defined in files SS_SymbolTable c SS_SymbolTable h and SS_Parameter c SS_InitSymbolTable Allocates and initializes an empty symbol table structure SS_PurgeSymbolTable Deallocates an initialized symbol table structure All the symbols in the table are discarded SS_NewSymbol Creates a named symbol and allocates memory for the data structure SS_GetSymbol Looks up and returns a symbol from a symbol table indexed by the symbol name SS_PutSymbol Inserts a symbol into a symbol table The symbol structure must be properly allocated and initialized If a symbol of the same name already exists in the table then it is replaced by the newly inserted value 13 e SS_PutDoubleSymbol Creates and initializes a symbol structure with the real number value supplied This is a shorter and more convenient way of creating symbols if the user does not want to do the initialization of the symbol structure Uses SS_PutSymbol1 for insertion and hence checks for duplicates e SS_PutStringSymbol Similar to SS_PutDoubleSymbol inserts a string valued symbol into a symbol table 3 4 3 Parser and Initialization File Utility Modules This module handles the parsing of the initialization file Normally the programming of the model will not find this model useful in any way but we will include a brief description of its functionality in this section for completeness This module consists of the
59. s as well as the model initial conditions can be done through the initialization file Unless explicitly specified through the command line parameter SimSect rc is the default initialization file 2 4 1 File Format In SimSect various parameters and variables are defined as symbols whose values can then be accessed by both the integrator and the model specification to read user specified settings The initialization file consists of assignments to these symbols in the following syntax symbol_name value The symbol_name can be any string starting with a letter and including letters digits or the underscore character The symbols in SimSect can be of type real number or string The value field hence can be either a real number or a string enclosed in quotes Finally lines starting with the symbol are considered as comments and are ignored Figure 1 illustrates an example init file 2 4 2 Summary of Integrator Symbols This section summarizes the symbols predefined by the SimSect integrator subsystem Model specific symbol definitions will be discussed in conjunction with the details of each particular model see Sections 4 and 5 e systemName default hexapod The name of the dynamical model to be integrated Currently valid values are hexa pod akh and slip If an invalid value is specified the model defaults to hexapod This is a comment recordPeriod 0 01 measurePeriod 0 1 dataBaseName h
60. s function is 20 int ValidateChart int inChart double x double p int chart The inChart argument returns a flag indicating whether the given state is in the specified chart or not 3 5 9 The Auxiliary Functions The auxiliary function mechanism in SimSect is provided for the model programmer to be able to record arbitrary functions of the system state and output them to a file By use of this mechanism SimSect can directly output integrated trajectories in the desired coordinate system making it more convenient to process the output This also saves disk space and memory in cases where the variables of interest are very few compared to the number of system states The auxiliary function is stored in SS_Data auxFunc after model initialization The prototype for this function is int Auxiliary double f double x double p int chart This function fills in the array supplied in the argument f with all the computed auxiliary functions given the current state and the chart Note that the number of auxiliary functions remains the same throughout the integration and specified during the model initialization Exceeding that number in filling the output will most likely crash the system The computed functions can be anything from things as trivial as the current time to complicated coordinate transformations of the current state There is no restriction on what the model programmer defines as auxiliary functions as long as the n
61. such situations and ends up switching between two charts at very short time intervals Another possible modification to the current model is incorporating a different ground contact model Currently collisions with the ground are plastic and cause the small foot mass to lose all its energy This in turn results in step changes in the leg damping forces which sometimes puts the system in an invalid state chart combination In consequence the integrator fails to compute the trajectory Different ground models include high damping high stiffness ground contact models where the feet can penetrate the ground and loses its energy in a continuous fashion However this approach might lead to a more compli cated model which is undesirable for analysis purposes Moreover the high damping and stiffness coefficients for the ground may unacceptably slow down the numerical integration Modifications to the integration engine may be required References 1 J G A Back and M Myers A Dynamical Simulation Facility for Hybrid Systems DsTool documentation 2 P C Hughes Spacecraft Attitude Dynamics John Wiley amp Sons New York 1986 ew M Phillips Geomview Software Manual The Geometry Center 53
62. t form the flight phase whereas compression and decompression form the stance phase The model allows transitions between these modes only in the sequential order that is given above The transition from ascent to descent occurs when the vertical velocity of the body dur ing flight changes sign from positive to negative The transition from descent to compression occurs when the tip of the leg during descent touches the ground Then the system goes into decompression when the spring reaches maximal compression and it starts decompres sion Finally the ascent phase is reentered when the leg reaches maximal extension during decompression The boundary functions associated with each of these transitions are defined below ener Car y y Daesc comp Z Y Y qrt COS qo beomnodecomp l Ezy r bdecomp gt asel Y Qri qr where qrt Gr and qoi are the touchdown leg length liftoff leg length and the touchdown leg angles respectively Note that qr and q can easily be written as functions of x and y through a polar coordinate transformation In our simple model qg is actually used as a control variable We assume that during flight we are able to swing the leg to any desired angle relative to the ground which then determines when and at which configuration the leg touches the ground 4 3 The SimSect Implementation of SLIP This section describes the implementation of the SLIP model described above in SimSect Most of the example
63. t is then up to this function to decide whether this crossing corresponds to a boundary function and if so perform necessary changes to the state such as coordinate transformations discontinuous changes due to impacts etc and determine the new chart There are also instances where state changes occur even though the current chart does not change The prototype for the transition function is int Transition int trans int newChart double xnew double x double p int chart The arguments trans newChart and xnew must be filled out by the function trans indicates whether a chart transition has occurred This is used by the trajectory iterator to decide whether other function calls such as the DefineChart are required The newChart and xnew are the new chart and the new state after the transition and will be computed according to the particular aspects of the model 3 5 8 Validating a Chart This function is used by the integrator to determine whether the current state is valid for the current chart mainly for debugging purposes An invalid state corresponds to such cases where the partitioning of the state space prescribes a particular chart which is different than the current chart as imposed by the integrator This might correspond for instance to a case where the toe of a hopper is underground while the hopper is in the flight chart The SS_Data vcFunc variable points to this function after initialization The prototype for thi
64. te and chart defining the flow Usually this is the most complicated function in a model definition because it implements the dy namics of the model as well as other components such as controllers etc Naturally all the functionality does not need to be implemented in a single function body A more modular approach with multiple procedures is possible and should be considered when possible The prototype for this function is int VectorField double xdot double x double p int chart The array pointed to by the argument xdot must be filled in by the function with the computed vector field One of the major mistakes made in implementing a model is to use the vector field function to incorporate discrete changes in system structures from within the vector field The calls to the vector field function do not occur in chronological order Hence even if the integrator computes the flow at a particular time this does not necessarily mean that the integration has reached that point Consequently the assumption that the time which is one of the states usually that the flow is computed at can be used to make discrete decisions about the structure of the system is wrong The vector field should properly define a not necessarily continuous function of the state space without any other internally maintained discrete state It is up to the hybrid integrator together with the transition functions to perform those discrete structural changes to the
65. the previously defined stopping functions Consequently the definition of the chart involves redefining all the stopping functions to be used in the new chart The hex_Boundary defines all the boundary functions associated with the liftoff and touchdown conditions The indices corresponding to each of these functions are as follows Liftoff 2 leg_no Touchdown 2 leg_no 1 The chart definition then adds stopping functions with the liftoff boundary functions for legs in stance and with the touchdown boundary functions for legs in flight 5 4 5 Chart Validation In the hexapod model there are many cases where the state may be inconsistent with the current chart that the system is in The following list summarizes these cases and gives the action taken by the hex_ValidateChart function Essentially most of these conditions invalidate cases where one or more of the boundary functions associated with a chart have negative values corresponding to anomalous cases e For all charts if rp lt h foz rby then the body toppled over and is underground Return an error state to stop integration e For a leg in stance if p gt fo the leg length exceeded the liftoff length Display a warning and continue integration e For a leg in stance if ni fs fy Fi gt 0 the normal leg force is positive Return an error state to stop integration e For a leg in flight if 001 i ifs Pu Fes fy lt 0 the leg is underground Return an
66. this section we describe a four parameter family of open loop controllers for the hexapod model which achieves forward and backward running as well as in place and differential turning in the abscence of any sensory feedback of the rigid body and leg states The only feedback occurs locally at the actuators to implement a PD controller for each hip tracking certain periodic reference trajectories The algorithms that we describe in this section are tailored to demonstrate the intrinsic stability properties of the compliant hexapod morphology and emphasize its ability to operate without a sensor rich environment An alternating tripod pattern governs both the running and turning controllers where the legs forming the left and right tripods are synchronized with each other and are 180 out of phase with the opposite tripod as shown in Figure 8 42 left tripod A pa a aa aR ai a 2 right tripod Figure 8 The motion profiles for left and right tripods 5 2 1 Running The running controller s target trajectories for each tripod are periodic functions of time parametrized by four variables te ts and o The period of both profiles is te In conjunction with t it determines the duty factor of each tripod In a single cycle both tripods go through their slow and fast swing phases covering and 27 of the complete rotation respectively The duration of double support tq where all six legs a
67. try are specified with this function at the time of initialization 12 SS_FlowMemAdd Adds a data entry to the flow memory The internal implementation of the flow memory allocates heap memory in chunks of a predefined size and fills them in with the data supplied through this function Hence the user does not have to worry about space allocation and memory management SS_FlowMemGet Returns a particular data entry from the flow memory indexed by an integer The data entries are ordered chronologically by the order they are added into the memory by the SS_FlowMemAdd function SS_FlowMemDump Outputs the contents of a flow memory in an ascii file where each row corresponds to a single data entry with the appropriate number of elements This file can easily be imported by numerical computation packages such as it Matlab SS_FlowMemClear Clears all the data in a flow memory and deallocates the allocated heap memory 3 4 2 The Symbol Table Utility Module The symbol table module provides facilities for creating maintaining and accessing symbol tables Its main use in SimSect is to handle the interface between the definitions in the initialization file and different components of the system including the integrator itself as well as the model definition Symbols in SimSect are named variables which can hold either a real number or a string value Utility functions to setup symbol tables and to create and access symbol
68. umber of functions is correctly specified 3 5 10 Geomview Interface and Visualization The interface of the model to Geomview involves two components the initialization and the update functions Typically the initialization function is called during the model initializa tion and is responsible from setting up the initial geometry The integrator does not make any function calls specifically for the initialization of Geomview apart from its invocation and resetting It is up to the model to initially configure Geomview geometry and visualization options The second function is the one pointed to by the variable SS_Data geomFunc after the model initialization This is the the function which updates Geomview at a particular fram erate by sending the updated transformation matrices to Geomview The prototype for this function is int UpdateGeomview double x double p int chart This function is called by the flow iterator at every time step Most often the model programmer defines a model parameter frameRate which determines the period with which the Geomview updates are sent out Then this function usually computes the transformation matrices for various objects in the geometry and send them to Geomview through the use of the SS_GeomviewSend utility function Moreover a common practice is to enclose the update commands in the following form 21 i Us Figure 4 The spring loaded inverted pendulum SLIP leg model int

User`s Manual - Electrical Engineering and Computer Science

Contents

Download Pdf Manuals

Related Search

Related Contents