Visualizing and simulating a serial linkage defined using ...mate.tue.nl/mate/pdfs/10633.pdf ·...

Visualizing and simulating a serial linkage definedusing DH-parameters. (DCT 2009.039)

Erik Hijkoop (0575617)

1 September 2008 - 30 November 2008

Internship Mechanical Engineering DSD CSTSupervisor UVic Ass.prof D. ConstantinescuSupervisor TU/e Prof.dr.ir. M. Steinbuch

Contents

1 Thanks to 3

2 Introduction 4

3 Purpose of the work 5

4 Description of the 6DOF haptic interaction system 64.1 Communcation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64.2 Simulation and visualization . . . . . . . . . . . . . . . . . . . . . . . . . . 8

5 Communcation 95.1 Solution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

6 Virtual Environment 156.1 CMLabs Vortex . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156.2 Denavit Hartenberg Convention . . . . . . . . . . . . . . . . . . . . . . . . 156.3 C-file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 166.4 Exceptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

7 Miscellaneous 20

8 Future work 21

9 Discussion 22

10 Conclusion 23

11 Bibliography 24

A Thing to know before running the equipment 25

B Running the equipment 26

C Work report Quanser 27

D Source code c-file 37

E Source code c-file Mass Matrix 55

2

1 Thanks to

I enjoyed my three months internship in Victoria. Without some help it had been impos-sible for me to go to Canada. So I would like to thank some people and agencies for theresupport. First of all I would like to thank the department of mechanical engineering at thetechnical university of Eindhoven for their financial support. Next to them I would liketo thank the student service centre for granting me the ECTS fund. Althought money isimportant I wasn’t going anywhere without the help of Maarten Steinbuch, so thanks forgiving me the support and the contacts. Of course I would like to greatly thank DanielaConstantinescu for her support and giving me the possibility to do my internship at herhaptic lab. Next to that I would like to thank my family and friends for sending me emailsand supporting me with my plans.

3

Figure 1: Controllers and the virtual and real proxy.

2 Introduction

In this report the work done during my internship is described. At the University ofVictoria (UVic), Daniela Constantinescu runs a lab where research in the haptics takesplace. Haptics is the field of feeling, realistic, forces generated in a virtual world due tothe movements of the end effector of the haptic device. The end effector is the part ofthe haptic device held by the user as shown in Figure 1. In our case a Quanser 6 DOFHaptic Wand serves as the haptic device. This device is capable of sensing the rotationsand translations of its end effector about all three axes. Furthermore, a force sensor isinstalled on the end effector so forces and torques in all directions can be measured too.This feature will be implemented in the future.

This report serves two purposes: (1) it describes the problems encountered during myinternship; and (2) it provides a reference for the next person working on the system. Tounderstand the report some knowledge about C/C++ is necessary, as is some knowledgeabout Matlab and Matlab Simulink. The project uses Microsoft Visual C to build thevirtual environment, and Matlab Simulink to develop the control of the haptic wand.Therefore, it is handy if the next person working on this project is familiar with theMicrosoft Visual C programming environment.

The report starts by describing the state of the haptic interaction system at the be-ginning of my internship. Then, the purpose of the research is explained in Chapter 3.All parts of the system are detailed in Chapter 4. The changes in the communication be-tween the virtual environment (written in C) and the haptic device controller (developedin Simulink) are clarified in Chapter 5. Chapter 6 presents the new simulation/visual-ization environment. Thereafter, some small adjustments to the system are illustrated inChapter 7. Chapter 8 hints to future work, Chapter 9 presents a discussion of the workaccomplished, and the conclusions are drawn in Chapter 10.

4

3 Purpose of the work

The goal of haptics is to apply realistic forces to users during users’ interaction with avirtual environment. Several applications can be conceived, for example a training facilityfor doctors. For such a facility, the hardware would look very different from what is used inthe lab right now. Haptics can also find application in computer games, where perceptionis an important part of the experience with the virtual world. So improvements in theperception of the interaction would improve the virtual experience. Again, the hardwarecan have various shapes and appearances. In art and design, designers could change thevirtual product to have the right feeling if they could feel its texture. Next to that investorscould be convinced by feeling the new product without making a physical prototype.

Existing research in the haptics lab at UVic has mainly focused on 1 to 3 degrees offreedom (DOF), but not on 6DOF (i.e., spatial body interaction). Two Quanser 6 DOFHaptic Wands are now available in the lab. Before this project started in September 2008a Simulink program was built to calculate the position and the velocity of the virtual proxyin all directions. In addition, a C-file was available to simulate a robot linkage, and thecommunication between the haptic wand and the simulation was established.

5

Figure 2: Data flow between the 6 DOF haptic wand and the simulation.

4 Description of the 6DOF haptic interaction system

The haptic interaction system consists of a 6-DOF Quanser haptic wand, an I/O hardwarecard in the PC and associated software. The software has two main components: (1) theC-file that implements the simulation and the visualization of the virtual environment; and(2) the Matlab Simulink file which comprises I/O and control algorithms for the hapticdevice. The communication between the haptic device and the virtual environment requiresa component in the C-file, as well as a component in the Simulink file.

The 6-DOF Quanser haptic wand provides positions and angles in three directions andapplies forces and torques according to the forces and torques calculated in the virtualenvironment. At every motor (7 in total), the shaft angles are sensed and sent to thecomputer, where they are transformed to end-effector coordinates in a base frame. TheSimulink file then sends the end-effector position and rotation to the C-file at 1000Hz, usingshared memory. In the C-file, the difference in position and velocity between the deviceend-effector and the virtual tool is used for a virtual coupler, i.e., a PD-controller. Thevirtual coupling forces and torques are returned to the Simulink file via the shared memory,and are also applied to the virtual tool in the virtual environment. In the Simulink model,the virtual coupling forces and torques are transformed to currents, which are suitablyamplified before being applied to the 7 motors of the haptic device. The haptic loopruns at 1000 Hz to simulate rigid contacts realistically. However, the visualization andsimulation loop in the C-file runs at an unknown, unpredictable, and changing refreshrate. Chapter 5 describes how the two asynchronous data streams (from the haptic side tothe graphic side and vice versa) are coupled.

4.1 Communcation

The communication between the wand and the Simulink model runs at a frequency of1000Hz. This is in contrast to the simulation and visualization C-file, which runs at a

6

Figure 3: Illustration of the shared memory communication available at the start of thisproject. (Data send from the Simulink model to the c-file)

refresh rate that varies unpredictably between 10 and 100Hz. At the beginning of thisproject, shared memory communication between the haptic wand and the simulation hasalready been implemented. This prior shared memory-based communication mechanismis schematically represented in Figure 3. In the C-file, the 10th buffer entry is alwaysread, after which the memory block is flushed. Some preliminary testing revealed severalproblems with this communication approach.

Figure 4 illustrates typical input/output behavior of the prior shared memory-basedcommunication scheme. For testing purposes, the Simulink model writes an ascendingnumber to the memory. The C-file reads this number and writes it back to the sharedmemory after a preset delay. Finally, the memory is read again by the Simulink model. Ineffect, the C-file is a direct throughput with a present delay.

As shown in Figure 4, two problems occur with this pre-existing communication betweenthe Smulink models and the virtual environment:

• A significant delay occurs when sending data packages smaller then 19 bytes (see

7

Figure 4(a)). The delay can be as big as 5 seconds. No explanation is neededto clarify that such long delay definitely destroys the realistic feeling and even thestability of the interaction.

• In Figure 4(b) the second problem is illustrated. Here the output drops to zero atunpredictable moments. This denotes the fact that no data is read from the sharedmemory at those moments.

(a) 1 byte send and 10 delay added (b) 100 bytes send and 0 delay added

Figure 4: Issues in the Input/Output behavior of the simulink model with the virtualenvironment.

These two problems with the shared memory-based communication between the hapticwand and the virtual environment are quite severe, especially because stability and physicalrealism are important in haptics. Solutions to these problems are given in Chapter 5.

4.2 Simulation and visualization

For testing purposes a C-file was available. This C-file implements the virtual environmentcomprising a hard coded virtual linkage, as shown in Figure 5. The C-file uses Vortex tosimulate and visualize the virtual linkage. Vortex is a physically-based simulation enginedeveloped by CMLabs. This engine has several options to automatically simulate and/orvisualize virtual objects. Hence, it could be compared with Lego, although the simulationneeds to be told the properties of the blocks, such as weights, weight distributions, shapes,positions and mass moments of inertia. The joints are build in a similar way. Furtherdetails about the virtual environment are provided in Chapter 6.

The cube shown as a separate object in Figure 5 represents the end-effector of thehaptic wand, i.e., the user’ hand. The connection between the virtual proxy and the endeffector of the wand is handled by a PD-controller based in the Simulink file, and is calleda virtual coupler. The virtual proxy is also connected with the center of the blue link, i.e.,to the virtual tool, by another spring damper connection.

8

Figure 5: Existing virtual environment.

5 Communcation

As shown above in Chapter 4.1, the data streams between the Simulink model and theC-file were handled incorrectly. Standard Quanser communication blocks were used inSimulink to receive data from and send data to the C-file. The existing problems havebeen solved during the internship and a work report is available in Appendix Chapter C.Herein only the end solution is discussed.

5.1 Solution

Figure 6 illustrates the desired behavior of the shared memory-based communication be-tween the haptic wand and the virtual environment. Specifically, the Simulink modelshould: (1) send data to the shared memory asynchronously; and (2) read it back with aconstant delay of maximum two steps of 1/1000 seconds delay (one delay for data sendingand one delay for data receiving). Furthermore, the size of the data blocks that need tobe exchanged between the haptic device and the simulation will vary with the application.Lastly, the data sent by the device to the simulation need not be of equal size to the datasent by the simulation to the device. Data sizes ranging from 1 byte to 500 bytes arepossible.

The new communications require only one extra thread in the C-file. This additionalthread is dedicated to the communications, while the other simulation thread handles allother calculations required for simulation and visualization (including force calculations).

As illustrated in Figure 7, the data sent by the Simulink model to the shared memoryis read by the C-file as soon as it is available in the memory. This behavior is guaranteed

9

Figure 6: Desired behavior of the communication.

10

Figure 7: The behavior of the new C-file, which handles the communications in a dedicatedthread.

11

as long as the communication thread in the C-file runs at a frequency of at least 1000Hz.This is always the case, because this thread only copies the data from model vector intothe data to collision vector, and the data from collision vector into the data to modelvector. Compared to the calculation thread this is a very low CPU demanding task.

Data synchronization, i.e., keeping all data with the same time stamp together, isrequired to prevent the memory from being updated before calculations are finished. Thefollowing sequence ensures that all data are synchronized:

• haptics data are read from the shared memory into data from model at the hapticsrate (every 1ms);

• simulation data are written to the shared memory from data to model at the hapticsrate (every 1ms);

• haptics data are read into data to collision from data from model at the hapticsrate (every 1ms);

• simulation data are written to data to model from data from collision at the hapticsrate (every 1ms);

• haptics data are read into a working variable from data from model at the (variable)simulation speed;

• simulation data are written to data from collision from a working variable at the(variable) simulation speed.

The problem of the output dropping to zero illustrated in Chapter 4.1 is solved bysetting a large enough size for the buffer in the shared memory. This size should be thenumber of doubles send times 8 bits. When problems occur, the buffer size should beincreased. This problem seems to happen predominantly when small amounts of data aresend to and from the buffer.

5.2 Results

The new implementation of the communications between the Simulink model and theC-file has improved the input/output behavior of the shared memory block significantly.Figure 8 illustrates that the delay with which the Simulink file reads the data back is 2ms,as expected. Furthermore, the new communications produce a smooth and predictablethroughput as shown in Figure 9.

The additional delay occurring in Figure 9(b) can be eliminated by not using the com-puter for other tasks while running the haptics application. This is because the computerthat hosts the haptics application runs Windows XP, and therefore, moving or clickingthe mouse and/or opening windows will add unpredictable delay to the communications.While the additional delay caused by the operating system may be small compared to the

12

Figure 8: There is 2 ms. of delay between the input and the output.

(a) Long term behavior. (b) Zoom-in.

Figure 9: The end behavior of the input/output of the shared memory.(1 byte data, 100delay steps in the simulation loop, 2ms sleep.)

13

variable refresh rate of 10 to 100 Hz of the virtual environment, it will nevertheless havesome influence.

Due to the operating system, the computer load influences the refresh rate of thecommunication. The communication thread runs much faster than 1 kHz, but its speedmay drop when another program is running. No drops below the 1 kHz mark have beenobserved during testing, possibly because the host computer is a dual core processor.

14

6 Virtual Environment

A virtual environment is required to test whether collisions can be suitably rendered viavarious controllers, like continuous impulsive force control [2]. The virtual environmentavailable at the beginning of my internship included a serial linkage with three hingedlinks. That virtual environment was not developed in a modular fashion and the linkagekinematics and dynamics were hard-coded. However, the virtual linkage simulation needsto be more general in the sense that it should allow a user to easily change: the numberof links; the type of joints, to allow both revolute and prismatic joints; the location of thebase joint in the virtual world; and the link which the user grabs the virtual linkage from.

6.1 CMLabs Vortex

The virtual environment is built using a physics simulation engine provided by CMLabscalled Vortex. Vortex uses the Microsoft C++ environment. Vortex can be used to imple-ment both the virtual environment simulation (i.e., collision detection, force computation,and time integration) and its visualization. The package provides primitive joints likeprismatic, revolute and ball joints that can be used to build other joints. It also providesprimite shapes like boxes, balls, and cylinders, as well as arbitrary meshes. Because thesimulation of meshes is much more computationally expensive than the simulation of otherprimitive shapes, the use of meshes should be avoided in haptics applications if possible.

6.2 Denavit Hartenberg Convention

In the new implementation of the virtual environment, the virtual linkage is described viaan extended Denavit Hartenberg[3] (DH) table. Specifically, the mass and the mass dis-tribution for each link are added to the traditional DH-table. The DH notation describesthe relative position and orientation of each link with respect to its predecessor link (i.e,the link closer to the base to which the current link is connected). In the implementationdescribed in this report, the frames are placed at the middle of each link. The DH no-tation is advantageous because it describes the 6DOF link position, i.e., translation androtation, with only two rotations and two translations. A further advantage is that thetransformation matrices from one link to the next are easily determined from the DH-table.According to the DH notation:

• The z0 axis is the joint axis.

• The xi axis is perpendicular to the axis z0.

• The xi axis intersects the axis z0.

• The yi axis makes a right handed frame with the other two axis.

As all frames are determined the two angles and two translations can be assigned easily.In Figure 10 the four DH-parameters are determined from two frames in 3D.

15

Figure 10: DH-parameters.

The transformation between two consecutive frames can be determined using the fol-lowing matrix:

Ai

cos(θi) −sin(θi)cos(αi) sin(θi)sin(αi) acos(θi)sin(θi) cos(θi)sin(αi) −cos(θi)sin(αi) asin(θi)

0 sin(αi) cos(αi) d0 0 0 1

This matrix results from multiplying four matrices in the following order: a matrix

representing a rotation of θi around the z-axis; a matrix representing a translation of di

along the z-axis; a matrix representing a translation of ai along the new x-axis; and amatrix representing a rotation of αi around the new x-axis.

6.3 C-file

The virtual environment is generated in the C-file, starting with the virtual world andcontinuing with the serial linkage. The linkage is built by iteratively building each link.Therefore, only the building of one link is described herein.

During the initialization, the visualizer and the simulation need to be built separately.Upon initializing the visualizer (i.e., assigning a memory address to the frame data), aframe can be added. The simulation is initialized by creating a universe and adding it tothe visualizer. Upon initializing the visualizer and the simulation, the virtual mechanismcan be built. The virtual mechanism includes a distinguished base part, the ground, andan assembly. The assembly is a dynamic collection of parts, in this case the links andthe joints of the virtual mechanism. Upon initializing the visualizer (approximately 5 s

16

during which the first frame is shown frozen on the screen), the C program initializes thesimulation and finally starts the simulation thread (Line 1478).

Unless the initialization of the virtual mechanism is performed in the correct order, thesimulation crashes. Each part of the assembly has to store the position, orientation, massand other information in the right order, after which the assembly needs to be updated. Thestandard procedure for building a part is itemized below and all lines in the program areadded, which makes it easy to check the corresponding C-code. (See appendix Chapter D)

• Transformation matrices are calculated. (Line 764-789)

• Positions of the last frame and the new frame are determined in the base frame usingthe transformation matrices. (Line 882-920)

• The midpoint of the link is calculated. (Line 934)

• The orientation of the link is determined. (Line 938-978)

• The geometry of the link is calculated. (Line 409-431)

• A transformation matrix is created from link orientation and position. (Line 1023-1041)

• The part is created from the available data (mass, orientation, location, geometry).(Line 1043-1109)

• The part is added to the assembly. (Line 1131)

• The axis of the joint constraint is determined. (Line 1136-1137)

• The constraint is created using its position and z-axis. (Line 1141-1156)

• The constraint is added to the assembly. (Line 1155)

The virtual proxy and the handle of the haptic device are coupled through the virtualcoupler as mentioned earlier. The virtual coupler is a PD-controller that uses the proxyand the device positions and velocities. The Simulink model sends the position and thevelocity of the device to the C-file, while the position and the velocity of the virtual proxyare obtained from the simulation. The virtual coupling forces and torques are applied tothe virtual proxy in the C-file and sent through the shared memory and the Simulink fileto the haptic device. In the Simulink file, the virtual coupling forces and torques are scaledusing an extra gain for security reasons. However, this gain should not be used and theforces and torques should be sent directly to the equipment when the 6DOF haptic setupbecomes fully functional. In the old implementation, the positions of the god object [1]were reset to the positions read from the haptic device. This can result in discontinuousforces and torques which can trigger instability.

17

θ d a α R(1)/P(0) mass mass distribution0 0 3 0.5*pi 1 0.1 0.50.5*pi 0 0 0 1 0 0.50 5 0 0.5*pi 1 0.2 0.5

Table 1: DH-table example with dummy link

Figure 11: The use of a dummy link to show the fact that more then one joint is added tone point.

6.4 Exceptions

Several problems occur when the sequence described in Chapter 6.3 is used. One problemis that a link offset is required to allow the links to move over some range before collidingwith adjacent links. This link offset is implemented by shortening the link length in theDH-table by twice the value of the link offset. The link offset is currently equal to the linkthickness and thus, allows a large angle of rotation between adjacent links. However, thelink offset can easily be changed if larger or smaller relative angles between adjacent jointsare desirable, or if the virtual mechanism includes only prismatic joints.

Another problem is the requirement to use more that one 1DOF joint at one position,for example in order to allow the implementation of a spherical joint. In the DH-table, thiscan be done by adding a part with a = 0 and d = 0, such that no shape is assigned to thepart in the visualizer and the simulation, see Table 1. Such a link is a so-called dummypart. The first joint is added at the end of the first link. However, because the next “link”in the DH-table is a dummy link, it requires a dummy joint at its end. The dummy linkhas no weight and is not involved in collision detection. The dummy link is made visiblein Figure 11, as it is in the program. Since multiple dummy links can be added in thismanner at one position, joints with several DOFs can be created.

The ability to hold any link of the virtual linkage is easy to implement by setting the

18

index of the link that the user holds in the C-file. Basically, this index corresponds tothe column in the DH-table. This index can be bigger then the number of actual (visible)links because dummy links may exist. A warning is written to the command window if theuser chooses to hold a dummy link when dummy links exist. To provide visual feedbackregarding the link held by the user, this link changes in color from green to red as shownin Figure 11.

19

7 Miscellaneous

To allow the implementation of additional controllers, like computed torque control, thejacobian and the mass matrix of the virtual linkage are needed. Because C providesno linear algebra libraries, these functions have been developed as part of the currentproject. The program capable of calculating the mass matrix can be found in Chapter E.The source code also includes several functions that support matrix algebra calculations,including matrix vector multiplication, matrix matrix multiplication, matrix transposition,and determinant of 3X3 matrices. Hence, the first steps towards a complex control schemeare taken, although additional functions will have to be developed by upcoming students.

A hardware problem that my predecessor has experienced was the loosening of thejoints of the haptic wand. Specifically, the shaft holding the link of the haptic wand thatserves as its end effector is at a place where not sufficient for the forces applied by the motorinside the end effector. Luckily Quanser designed new joints and this problem should notcome up anymore.

20

8 Future work

After three months of work, there still is a lot to be done, including

• As an extension to the virtual environment, other virtual objects should be added.For example, walls and other moving objects should be present in the virtual envi-ronment. These objects should be added in the C-file, similarly to how the virtuallinkage is currently added to the simulation universe. Because separate mechanismswould be used, they could be switched on and off very easily.

• At the moment, collisions are possible and the collision detection and collision re-sponse procedures embedded into Vortex are used. But this is definitely not desired.So a collision response procedure should be written. This can and probably will in-clude continuous impulsive force control, which gives a very realistic feeling of hittinga wall.

• Computed torque control may become part of the low level control of the hapticwand. Just like the CIF-control, computed torque control needs matrix inversion.Therefore, a general purpose matrix inversion function should be written in C.

• The virtual coupler implementation has not been optimized. Only low gain valuesare used and an extra safety gain is implemented inside the Simulink model. Theexisting gains should be replaced via optimized gains, i.e., gains that ensure robuststability of the 6DOF haptic interaction with a virtual linkage. One way to determinethe optimized gains is via trial and error inside the C-file. For realistic force feedback,the positions of the wand end effector and of the virtual proxy need not be exactlythe same as long as the error is not too big. How big the error can be before itimpacts user’s perception is a question that requires further examination.

• The number of links of the virtual linkage still has to be set inside the C-file andseparately from the DH-table. This should be changed to avoid errors due to thefact that a user may change the virtual linkage by changing the DH-table and mayforget to correspondingly change the number of links in the C-file. One possible ideais to send the DH-table and the number of links from the Simulink model to the C-file, such that consistency between the Simulink model and the C-file is maintainedautomatically without the need for any user input.

21

9 Discussion

Of course not everything went well and sometimes I had some struggles. I am not acomputer science student, so I had to learn C/C++, which is a very powerful computerlanguage. So everything is possible but also every mistake is directly punished. I haveproduced a few C-files which are probably not the top of the line as we consider efficiency,but they work. However this was not always the case. The visualization and simulationC-file has to be dynamic as mentioned above. First I had a file which was dynamic buthad a lot of errors and therefore exceptions in it. I tried to fix the errors with a patch andbecause of that the file became impossible to maintain. Debugging became a mess. Soafter working on that file for at least a week and possibly more, I started over and the filepresented here is the result.

Something else that could have been done better was the naming of the huge amountof data produced, while testing the communication. I had never expected to have thisamount of testing data and plots, so it is now quite a challenge to find a particular plot.Also I could maybe have done more automatic testing, so I could have written a m-filewhich would have had produced a lot for me. In the end it would have saved me time.

Lastly, it has been challenging to find information in the Vortex documentation. Thisis why I started developing several functions which I later discovered were implementedin the package. I may not only have read the PDF version of the documentation but alsothe internet version of it. As an example, vector multiplications are available in Vortexwhen Vortex-defined types (VxVector3) are used to define vectors. I could have used theVortex-based vector algebra earlier had it have been easier to discover the Vortex-definedvector types earlier.

22

10 Conclusion

In the end the communication problems have been solved and the desired asynchronouscommunication method has been implemented. A general purpose simulation of a serialmechanism with an arbitrary number of links and with joints with arbitrary DOFs hasalso been developed. To simulate a desired mechanism, the user only needs to input itsDH-table. Furthermore, most hard-coded settings can be changed in lines 75 to 122. Herethe position as well as the orientation of the base part can be set, as well as the number oflinks and the shape and properties (thickness, offset, color, shape) of the links. The linkheld by the user can also be set in this first part of the file.

Two different linkages are shown in Figure 12. Both are generated using the sameC-program by changing the DH-table.

Figure 12: Two different linkages, generated by the same C program using two DH-tables.

23

11 Bibliography

References

[1] C.B.Zilles and J.K.Salisbury, A constrained-based God-object Method for Haptic Dis-play. Department of Mechanical Engineering, Artificial Intelligence Laboratorium, Mas-sachusetts Institue of Technology, Cambridge, Massachusetts, 1995.

[2] D.Hennekens, D.Constantinescu and M.Steinbuch, Continuous impulsive force con-troller for forbidden-region virtual fixtures. Department of Mechanical Engineering,University of Victoria, Victoria, Britisch Columbia, 2007.

[3] M.W.Spong, S.Hutchinson and M.Vidyasagar, Robot Modeling And Control. John Wi-ley and Sons, Inc, ISBN: 978-0-471-64990-8, 2006.

24

A Thing to know before running the equipment

Errors

1. One of the amplifiers does not turn to enable(green), when you have switched theamplifiers on and connect to target. Stop everything and rebuild the model, whenthis does not solve the problem, restart Matlab. Because this is a problem with thesimulink build.

2. The output in the command window gives an error that the transform is not correctof some parts. Check whether or not you have entered the DH-tabel correctly andset the number of links to the correct number of links.

Other

1. Where to find the readings of the haptic device?In the simulink model->Haptic Device(sub system)->Hil Interfacing(sub system). Atthe right you see all the measurements. The first scope are the forces and torquesmeasured at the force/torque sensor in the middle of the handle of the haptic device.The second scope are the encoder positions. The 3th scope is the last encoder positionwhich is empty. The 4th scope is the reading from the digital input/output. Which isbasicly not used, but implemented to test the digital IO.

2. Where to set the controller and why is the controller what it is now?The PD controller (Virtual Coupler) can be found in the C-File in the define settings.The controller has now very low P and D just to test the forces, the forces are scaleda lot lower where the loop is closed in the simulink file-¿controller(sub system). Justas a safety margin.

25

B Running the equipment

Running the visualizer and simulation of a robot build by the DH parameters.Startup:

1. Start Matlab

2. Run C:/EriksProject(02-09-08=28-11-08)/CfileAndModel/Model/running6DOF.mDon’t mind about the error that it can not plot some data.

3. Open C:/EriksProject(02-09-08=28-11-08)/CfileAndModel/Model/......communication haptic side controllerv1.mdl

4. Build this file (CTRL-B)

5. Open C:/Vortex/Vortex/src/tutorials/......Communication DH simANDvis/Communication DH simANDvis.vcproj (or the short-cut found in C:/EriksProject(02-09-08=28-11-08)/CfileAndModel/Cfile)

6. Press F5 to run the file. It should give a visualizer with the last set robot and ittries to communicate with the model. So in the command window you will receivean error saying that it couldn’t connect to the model.

7. Go to the Simulink model and ¡connect to the target¿ (CTRL-T)

8. Start the real time code, found in Simulation.

9. Go back to the c-file and hit F5. Now it tries to communicate again and it gives theforces and torques applied to the Held Link back. These forces and torques are sendto the model as well as to the virtual linkage.

Stop:

1. Close the visualization by pressing ESC.

2. Close the command window by pressing ENTER.

3. Stop the communication in the Simulink model using ¡stop real time code¿

4. Disconnect from the target. (CTRL-T)

Restart:

1. Rebuild the model to reset the start position to zero.

2. Redo Startup: [5-9]

Keep your hands on the red button, for when the system becomes unstable!

26

C Work report Quanser

27

Status & Difficulties Quanser Haptic 6 DOF Device

Erik Hijkoop (University of Victoria)

October 22, 2008

1

Contents

1 Part I - Executive summary 3

2 Part I - UVic requirements 4

3 Part I - Current approach 53.1 Haptics side of the communications . . . . . . . . . . . . . . . . . . . . . . 53.2 Simulation side of the communications . . . . . . . . . . . . . . . . . . . . 73.3 Testing methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

4 Part I - Issues with the current approach 94.1 Experimental data for various sizes of data packets . . . . . . . . . . . . . 94.2 Experimental data for various simulation rates . . . . . . . . . . . . . . . . 13

5 Part I - Conclusion 13

6 PART II - Two-threaded simulation solution proposed by Quanser 156.1 Desired behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 156.2 Observed behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

7 PART III - Two-threaded simulation with 4 global variables (Quanser) 257.1 Observed behavior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 267.2 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

8 PART IV - Details of final implementation of the wand-simulation com-munications (two-threaded architecture with 4 global variables) 32

9 PART V - Contacts 339.1 Quanser . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 339.2 University . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

2

1 Part I - Executive summary

UVic has acquired two 6DOF Haptic Wands that need to be connected to virtual environ-ments with variable frequencies - typically ranging from 15Hz to 1000Hz, and occasionallydropping as low as 5Hz. The connection is to be implemented using the the stream serverblock in the communications API provided by Quanser. Because the documentation of thestream server block and the earlier suggestions of Quanser’s engineering team have notbeen particularly helpful to the student who worked on connecting the haptic wand to anexemplary virtual environment, this report details:

• the UVic requirements for the communication between the haptic wand and theinteractive simulation.

• how this communication has been implemented using Quanser’s communicationsblock.

• what issues we have encountered with the current implementation.

The goal of the report is to clarify our difficulties to your team, such that they can adviseus how the wand can be connected to interactive simulations as per our requirements.

Specifically, we are looking for advice on:

1. how a simulation loop developed as a Windows console application writtenin C can read the latest data from the haptic wand. The simulationfrequency can vary from one step to the next from 5Hz to 100Hz, and isunpredictable. The size of the packets sent from/to the simulation by thehaptic device may vary from 1 byte to 500 bytes.

2. how we can modify our current testing scenario (Section 3.3) to checkthat the communications behave as per our requirements.

3

Figure 1: Data flow between the 6DOF haptic wand and the interactive simulation.

2 Part I - UVic requirements

At the University of Victoria, the Quanser 6 DOF Haptic Wand is used to add forcefeedback to an interactive virtual environment. The virtual environment is simulatedunder Windows on a dual-core processor computer. Depending on the application, varioustypes of information must be exchanged between the simulation and the haptic device(positions, velocities, forces, geometry of various objects, etc), according to the followingprotocol (see Figure 1):

• The simulation sends data to the haptic device at the completion of each simulationstep - asynchronously, at rates varying between 15Hz and 1000Hz. Therate variation is due to the variable complexity of the collision detection & dynamicresponse algorithms and cannot be predicted in advance. Upon sending data to thehaptic device, the simulation needs to receive the most recent data from thehaptic device.

• The haptic device polls the communications for new data every millisecond. Ifnew simulation data are available, the haptic device reads these data and updates itscontrol loops accordingly. The wand can send data to the simulation:

– synchronously with the haptic control loop (i.e., every millisecond);

– asynchronously, only in response to the new data received from the simulation.

Either method is suitable provided the simulation reads the most recent data sentby the device.

The size of the data blocks that need to be exchanged by the haptic device and thesimulation will vary with the application. Furthermore, the data sent by the device to thesimulation need not be of equal size to the data sent by the simulation to the device. Datasizes ranging from 1 byte to 500 bytes are possible.

4

Figure 2: Simulink diagram of the haptic control loop.

3 Part I - Current approach

To implement the communications as per the requirements in Section 2, we have imple-mented the following approach:

• Have the haptic device and the (slow) simulation exchange data via shared memory,with the haptic loop being the server and the simulation being the client.

• Define a shared memory space for the 6DOF wand of size equal to the size of the datapacket sent by the device to the simulation. Our thinking is that if the shared memoryheld only one haptic data packet, the haptic loop would continuously overwrite thememory. Effectively, only the latest haptic data would be available to the simulationto read whenever it completed a simulation loop.

The following sections detail our current implementation of these communications andof the procedure used for testing them.

3.1 Haptics side of the communications

The wand control loop is implemented via the Simulink model shown in Figure 2. In thisSimulink model, the communications are implemented via Quanser’s stream server block(SSB).

The following settings are chosen for the SSB(see Figure 3):

• The en-input is 1.

• URI is shmem://foobar:1, which is similar to the address in the C-code.

• Send buffer size is 8 times the packetsize (8*30 bytes).

5

Figure 3: Settings of the stream server block (SSB) in the Simulink model in Figure 2.

6

• Receive buffer is 8 times the packetsize (8*30 bytes).

• Byte ordering is little endian (the smallest data pieces will be sent first).

• Optimize for is minimum latency, i.e., data is fed through as soon as it arrives at theSSB input.

• Implementation is use blocking I/O in separate thread, i.e., separate threads are usedfor sending and receiving.

• Send options is send most recent data, i.e., only the newest Simulink data will bewritten to memory. This data arrive at the fixed frequency of the haptic control loop.

• Receive options is receive most recent data, i.e., only the newest simulation data areread through the rcd port of the SSB. The newest data arrive at the variable rate ofthe simulation.

• Send fifo size in samples is 1 in order to collect and simultaneously send to memorythe haptics data (i.e., this should minimize latency to 1ms, the sample time of thehaptic loop).

• Receive fifo size in samples is 1 in order to receive from memory and simultaneouslysend simulation data to Simulink (i.e., this should minimize latency to 1ms, thesample time of the haptic loop).

• Send thread priority is 0.

• Send thread priority is 0 too, i.e., both threads have equal priority.

• Default output value is zeros(1,packetsize) (i.e., a vector of zeros of the length of theoutput vector generated by the SSB).

• Sample time is qc get step size, i.e., the sample time of the Simulink model (0.001seconds).

• Output data type is determined by the output channel (doubles are expected).

3.2 Simulation side of the communications

The virtual environment simulation is a Windows application, written in C using a physics-based engine. It comprises the collision detection, the collision response and the visualiza-tion algorithms. The simulation side of the communications is implemented via:

• Including the Quanser headers in the main c-file:

#include "quanser_stream.h"

#include "quanser_messages.h"

7

• Defining constants:

#DEFINE SYNCHRONIZER 9

#DEFINE NUM_OF_ELEMENTS 30

• Initializing the communications and defining the size of the data packets:

const char uri[] = "shmem://foobar:1";

const t_boolean nonblocking = false;

/* size of data packet to be sent by the simulation to shared memory */

const t_int send_buffer_size = 8*NUM_OF_ELEMENTS;

/* size of data packet to be read by the simulation from shared memory */

const t_int receive_buffer_size = 8*NUM_OF_ELEMENTS;

const char * locale = NULL;

t_uint numofelements = NUM_OF_ELEMENTS;

t_double valuesin[NUM_OF_ELEMENTS];

t_double valuesout[NUM_OF_ELEMENTS];

t_stream client;

t_error result;

char message[512];

stream_connect(uri, nonblocking, send_buffer_size,

receive_buffer_size, &client);

The sizes of the sent & received data packages have equal size in the current im-plementation. This is for debugging only. In the final implementation, the sent &received packets will have different size.

• In each simulation step:

– Skipping the oldest data in the shared memory:

stream_ignore(client, 8*SYNCHRONIZER*NUM_OF_ELEMENTS);

– Reading data from shared memory:

stream_receive_double_array(client, &valuesin[0], NUM_OF_ELEMENTS);

– Uploading data to shared memory:

8

stream_send_double_array(client, &valuesout[0], NUM_OF_ELEMENTS);

– Emptying the shared memory:

stream_flush(client);

The rate of the simulation loop is unpredictable and varies from one step to the next1.

3.3 Testing methodology

Whether the simulation reads the most recent haptics data from the shared memory hasbeen tested via the following procedure:

• The haptics loop has sent data to shared memory at a rate of 1ms.

• The virtual environment has read data from, and has written data to, the sharedmemory at the beginning of each simulation step. Upon reading from/writting tomemory, an idle FOR loop has mimicked the collision detection and response algo-rithms. Effectively, changing the duration of the FOR loop has changed the frequencyof the virtual environment.

4 Part I - Issues with the current approach

Using the protocol described in Section 3.3, we expect the haptics loop toread back the data that it sends to shared memory, sampled at the simulationfrequency.

However, the experimental data that we collected demonstrate a different behavior ofthe communications. These data were collected by: (a) varying the size of the data packets;and (b) varying the rate of the virtual environment. They are presented in the followingtwo sections.

4.1 Experimental data for various sizes of data packets

The size of the data packets has been selected via setting the following parameters to thetested value:

• the send/receive buffer size parameters in the SSB.

1This is because the computational time of the collision detection and collision response algorithmschanges from step to step depending on user’s actions.

9

• the input (snd) of the SSB.

• the NUM OF ELEMENTS constant in the C-file.

We tested packet sizes ranging from 1 byte to 500 bytes. However, we only present datawe collected for four packet sizes - 1 byte(i.e., 1 double), 20 bytes, 100 bytes and 500 bytes- because these data exhibit all the trends we observed. Specifically:

1. when sending packets smaller than 19 bytes, the data sent by the haptic wand isread back with a severe delay. This is shown in Figure 4 for a packet size of 1 byte.For comparison, Figure 5 shows the data written to/read from the shared memoryby the haptic wand for a packet size of 20 bytes.

(a) 1 byte send and 0 delay steps added (b) 1 byte send and 10 delay steps added

(c) 1 byte send and 100 delay steps added (d) 1 byte send and 1000 delay steps added

Figure 4:

2. when sending packets larger than 100 bytes, no data is read from shared memoryat some moments - note how the scope in Figure 6 shows the data dropping to zerosometimes.

10



(e) 20 byte send and 100000 delay steps added

Figure 5:

11



Figure 6:

12

4.2 Experimental data for various simulation rates

The speed of the simulation has been selected via the following code (added to the mainloop of the C-file):

int r=0;

int d;

for (d=0; d<r; d++) printf("%d \n ", d);

The code has been inserted just before the simulation loop reads the haptics data from theshared memory:

stream_receive_double_array(client, &valuesin[0], NUM_OF_ELEMENTS);

Various values of r will result in different simulation speeds. We have tested values rangingfrom 0 to 100000 steps, i.e., from 0s to 4s (approximately). In our tests, the simulationdelay seems to have little effect on the data read by the wand from shared memory forlarge packet sizes. However, for large simulation delays and small packet sizes, the wand nolonger reads back the data it sends to the simulation (see Figure 5(e)). We have observedthe behavior illustrated in Figure 5(e) for other packet sizes, too, but are not presentingthe corresponding experimental data herein for brevity.

5 Part I - Conclusion

This report documents:

• UVic’s requirements for connecting Quanser’s 6DOF haptic wand and to a virtualenvironment simulation with variable speed;

• the current implementation of this connection using Quanser’s API stream serverblock and data exchange via shared memory;

• the behavior of the current implementation for various sizes of the data packetsexchanged between the haptic device and the simulation, and for various simulationspeeds.

We look forward to your engineering team advising us how to overcome our currentdifficulties.

13



Figure 7:

14

6 PART II - Two-threaded simulation solution pro-

posed by Quanser

In response to the first part of this report, Quanser suggested that the virtual environmentbe implemented using two threads: (1) one thread dedicated to the communication withthe haptic wand, i.e., to the (computationally light) sending & receiving of the data; and (2)another thread for the (computationally intensive) collision detection & collision responsealgorithms. This section presents the desired & actual data exchanged between the hapticand the graphic loops when the virtual environment uses this two threads architecture.

6.1 Desired behavior

In each step, the graphics loop should:

1. read the most recent data uploaded by the haptics loop to the shared memory;

2. upload these data back to the shared memory right away.

Because the graphics loop is slower than the haptics loop (15-100Hz versus 1000Hz), thesecommunications should effectively downsample the haptics data before returning themto the wand. Hence, the scope in the Simulink model should display two signals:

1. the haptics data;

2. the haptics data down-sampled at the frequency of the graphics loop.

This expectation is schematically depicted in Figure 8 for communications with a simulationwith variable frequency. Because we fix the frequency of the graphics loop, the hapticsdata should be down-sampled at a fixed frequency in our tests.

The following section presents the data that the scope in the Simulink model reads forvarious testing conditions.

15

Figure 8: Schematic representation of the data that the haptics loop should receive fromthe graphics loop.

16

6.2 Observed behavior

The same procedure as in Section 3.3 has been used for testing. The introduction of twothreads in the C-file requires one more parameter to be tested, the sleep parameter2. Wetested the data flow for:

1. various amounts of data being exchanged between the haptics and the graphics loops(implemented as various buffer sizes): 1 byte, 10bytes, 50bytes, 500bytes;

2. various speeds of the graphics loop (implemented via various delay times in this loop):0 delay steps, 100 delay steps, 100000 delay steps;

3. various sleep times: 0.002s, 0.01s, 0.05s.

Table 6.2 shows all conditions tested.Bytes Delay steps Sleep Bytes Delay steps Sleep Bytes Delay steps Sleep

1 0 0.002s 1 0 0.01s 1 0 0.05s10 0 0.002s 10 0 0.01s 10 0 0.05s50 0 0.002s 50 0 0.01s 50 0 0.05s

500 0 0.002s 500 0 0.01s 500 0 0.05s1 100 0.002s 1 100 0.01s 1 100 0.05s

10 100 0.002s 10 100 0.01s 10 100 0.05s50 100 0.002s 50 100 0.01s 50 100 0.05s

500 100 0.002s 500 100 0.01s 500 100 0.05s1 100000 0.002s 1 100000 0.01s 1 100000 0.05s

10 100000 0.002s 10 100000 0.01s 10 100000 0.05s50 100000 0.002s 50 100000 0.01s 50 100000 0.05s

500 100000 0.002s 500 100000 0.01s 500 100000 0.05s

Example data collected during these tests are shown in Figures 10 to 21. The figuresshow the overall picture on the left and a zoomed picture on the right. The experimentaldata depicted in these figures are selected to illustrate that the haptics loop does not readfrom the shared memory what it needs to read. Additionally, we noticed that the CPUusage is 100% for both cores when the communications are running, regardlessof the amount of data exchanged by the haptics and graphics loops, the delayof the simulation, and the value of the sleep parameter.

We look forward to your advice regarding how the communications block can be used toimplement the communications as per the requirements outlined in Section 2 and depictedin Figure 8.

2The sleep parameter is used to set the period of time during which the graphics loop sends data tothe haptics loop.

17

Figure 9: Schematic representation of the data that the haptics loop should receive fromthe graphics loop (zoomed).

18


Figure 10: 1 byte data, 0 delay steps in the simulation loop, 0ms sleep.



19



(a) Long-term behavior. (b) Zoom-in.


20





21

(a) Long-temr behavior. (b) Zoom-in.

Figure 16: Experimental data for: 50 byte data, 100 delay steps in the simulation loop,50ms sleep.



22





23





24

Figure 22: Implementation of communications with 4 global variables.

7 PART III - Two-threaded simulation with 4 global

variables (Quanser)

In response to the difficulties experienced when the communications were implementedin dedicated thread in a multi(two)-threaded simulation, Quanser have suggested a two-threaded implementation of the simulation that uses 4 global variables: data from model,data to collision, data from collision, data to model. In this solution:

• haptics data is read from the shared memory into data from model at the hapticsrate (every 1ms);

• simulation data is written to the shared memory from data to model at the hapticsrate (every 1ms);

• haptics data is read into data to collision from data from model at the (variable)simulation speed;

• simulation data is written to data to model from data from collision at the (variable)simulation speed.

These communications are schematically depicted in Figure 22.

25

7.1 Observed behavior

We again tested the new files for the parameter values in Table 6.2, with a single exception:we tested 1000 delay steps instead of 100000 delay steps. We did this because 100000 delaysteps translated into a non-interactive simulation3, i.e., a simulation unsuitable for hapticinteraction. Sample results are plotted in Figure 23 to Figure 34. Conclusions drawn fromthese tests are presented in Section 7.2.



7.2 Conclusion

The results obtained with the new implementation are very close to what we would liketo see, although small deviations of the simulation frequency from the expected fixedfrequency can still be observed. Note in particular the results in Figure 23, Figure 26,Figure 30, Figure 25, Figure 26, and Figure 27. Figure 32 and Figure 33 show the behaviorwe would like to see.

The variations in the simulation frequency seem to be due to the fact that the C-file runs as a Windows application. Specifically, the simulation loop will be slower whencompeting for processor time with more processes than when competing with less processes.For example, the simulation frequency decreases when the user moves the mouse, opensa scope, etc. Hence, these variations of the simulation frequency seem to be unavoidable.Nonetheless, they will have no impact on the wand-simulation communications if theyare small compared to the time required by the collision detection & collision responsealgorithms. We expect this will be the case.

3For 100000 delay steps, the simulation loop was of the order of 2-10s.

26





27





28




Figure 29: Experimental data for: 1 byte data, 1 delay steps in the simulation loop, 10mssleep.

29





30





31



8 PART IV - Details of final implementation of the

wand-simulation communications (two-threaded ar-

chitecture with 4 global variables)

During the testing reported herein, Quanser have suggested some parameter values whosechoice they have not explained. This section documents these values.

• The value of the buffer in the streamserver block was set by default to 1460 bytes.This value can be adjusted, and it should be the number of doubles ∗ 8. For example,the buffer has been set to 4000 when sending 500 doubles from the wand to thesimulation.

Only sent & received data streams of equal size have been tested.

The streamserver block does not work correctly for very low values of the buffersize (for example, 8). We have not identified the lower bound for correct behavior.Rather, the buffer value was set to the default 1460 value when the block misbehaved.

• During testing, the time of the simulation loop has been set through adjusting thenumber of loops in:

int numofdelay = 100;

int l;

for (l=0;l<numofdelay;l++)

{

printf("\n Delay %d",l);

}

32

This loop runs at the maximum frequency possible. Therefore, it slows down whenthe user does something else on the PC in parallel with it (for example, opens anotherwindow). It follows that a haptics demo will run optimally only if the PC is fullydedicated to it.

• The sleep function adjusts the time of the simulation loop differently. This functionwill hold the simulation as long as the delay (in ms) set via the sleep parameter. Forexample, sleep = 50 lengthens the simulation loop by 50ms.

• In the C-file 4 global vectors are used. This is done to be sure that data doesn’tget mixed up with each other and you always have the latest data available whena simulationstep starts. At a high frequency the first connection loop reads thedata from the memory and writes it to the data from model vector. After that thisvector is copied in the data to collision vector. This vector is used in the collisionthread. Herein the vector can be used for all kind of calculations. Also the vectordata from collision has to be build here. This vector will be used to be copied againin the communication thread to the data to model vector. These values of the vectorare send back to the model as long as these values aren’t replaced by new valuescalculated in the collision thread.

9 PART V - Contacts

9.1 Quanser

• Kaveh Jenab [[email protected]]Kaveh has helped us directly with the communications problems. He provided thesolution described in this report.

• Paul Karam [[email protected]]The first contact for requiring technical assistance from Quanser.

9.2 University

• Daniela Constantinescu [[email protected]]Supervisor/Director of the Assistive Robotics and Mechatronics Lab at the Universityof Victoria.

• Erik Hijkoop [[email protected]]/[[email protected]]Dutch student at the Technology University of Eindhoven who has worked on theQuanser 6 DOF haptic device during his internship at the University of Victoria.

33

D Source code c-file

12 /∗3 C−f unc t i on : proxyimplementDH45 Desc r ip t i on : This f i l e s imu la t e s and v i s u a l i s e s a robot us ing as input the DH−parameters .67 Known bugs : −89 Authors : Under supe rv i s i on o f Danie la Constant inescu (UVic ) ( danie lac@uvic . ca )

10 Erik Hijkoop ( e . g . hi jkoop@student . tue . n l )1112 Inputs : DH−parameters13 Use Denavit Hartenberg notat ion f o r the d i s c r i p t i o n o f the robot .14 −Def ine z1 as the ax i s as the j o i n t ax i s15 −Def ine x1 perpend i cu la r to the z0 ax i s AND l e t x1 i n t e r s e c t the z0 ax i s .16 −Def ine y1 as the r ighthand coord inate frame1718 a : d i s t ance from frame i−1 to frame i over the x i ax i s19 d : d i s t ance from frame i−1 to frame i over the z i−1 ax i s20 a l f a : ang le between z i−1 and z i over x i21 theta : ang le between xi−1 and x i over z i−122 RP : Revolute (1 ) or Pr i smat ic (0 ) j o i n t23 mass : the mass o f the s e v e r a l l i n k s2425 Outputs : V i s u a l i z e r and the s imula t i on va lues o f the l i n k s . These va lues can be used f o r a

c o n t r o l l e r between the r e a l machine and the v i r t u a l environment2627 Date : 11−21−082829 ∗/3031 /∗ DEFINE AND INCLUDE ==========================================================================∗/3233 /∗ Inc lude VX part s ∗/34 #inc lude ”Vx/VxPrec is ion . h”35 #inc lude ”Vx/VxFrame . h”36 #inc lude ”Vx/VxBox . h”37 #inc lude ”Vx/VxColl is ionGeometry . h”38 #inc lude ”Vx/VxPart . h”39 #inc lude ”Vx/VxHinge . h”40 #inc lude ”Vx/VxPrismatic . h”41 #inc lude ”Vx/VxUniverse . h”42 #inc lude ”Vx/ VxConstra intContro l l e r . h”43 #inc lude ”Vx/ VxVisua l i ze r . h”44 #inc lude ”Vx/VxSpring . h”45 #inc lude ”Vx/VxVector3 . h”46 #inc lude ”Vx/VxBox . h”47 #inc lude ”Vx/VxTransform . h”48 #inc lude ”Vx/VxEventSubscriber . h”49 #inc lude ”Vx/VxPlane . h”50 #inc lude ”Vx/VxContactPropert ies . h”51 #inc lude ”Vx/VxConvexMesh . h”52 #inc lude ”Vx/VxRequest . h”53 #inc lude ”Vx/VxAssembly . h”54 #inc lude ”Vx/VxCylinder . h”55 #inc lude ”Vx/VxSphere . h”56 #inc lude ”Vx/VxBallAndSocket . h”57 #inc lude ”Vx/VxUniversal . h”58 #inc lude ”Vx/VxDoubleHinge . h”59 #inc lude ”Vx/ VxCyl indr ica l . h”6061 /∗ Inc lude Quanser communication∗/62 #inc lude ” quanser stream . h”63 #inc lude ” quanser messages . h”6465 /∗ Inc lude standard ∗/66 #inc lude ” s tda fx . h”67 #inc lude ”windows . h”68 #inc lude ” s td i o . h”69 #inc lude ” proce s s . h”70 #inc lude ” ctime”71 #inc lude ” fstream”72 #inc lude ” s t r i n g . h”73 #inc lude ”math . h”74 #inc lude ” crtdbg . h”7576 /∗ Delay and sk ip part77 Number o f doubles that t h i s app l i c a t i on r e c e i v e s from the hap t i c app l i c a t i on ∗/78 #de f i n e NUM OF ELEMENTS 307980 /∗ s c a l i n g length from the g raph i ca l s i d e to the hapt i c dev i ce ( So when LENGTH SCALE ON GRAPHICS i s

de f ined as 10 the g raph i ca l s i d e moves 1 cm as the hapt i c dev i ce moves 10 cm) ∗/81 /∗ SCALING IN DH TABLE i s in CM, in VORTEX SI i s used∗/82 #de f i n e LENGTH SCALE ON GRAPHICS 150

37

8384 /∗ PD c on t r o l l e r between the proxy and the l a s t l i n k ∗/85 #de f i n e P LINEAR 10 /∗N/m∗/86 #de f i n e D LINEAR 1 /∗Ns/m∗/87 #de f i n e P ROTATION 0 /∗N/rad Have to get the minus out somewhere∗/88 #de f i n e D ROTATION 0 /∗Ns/rad∗/8990 /∗Def ine the r o t a t i on and po s i t i o n o f the middle o f the basepart ∗/91 #de f i n e POS BASE X 1 0 . 5 ;92 #de f i n e POS BASE X 2 0 . 8 66 ;93 #de f i n e POS BASE X 3 0 ;9495 #de f i n e POS BASE Y 1 −0.866;96 #de f i n e POS BASE Y 2 0 . 5 ;97 #de f i n e POS BASE Y 3 0 ;9899 #de f i n e POS BASE Z 1 0 ;

100 #de f i n e POS BASE Z 2 0 ;101 #de f i n e POS BASE Z 3 1 ;102103 #de f i n e POS BASE X 0104 #de f i n e POS BASE Y 5105 #de f i n e POS BASE Z 0106107 /∗ Set Colors ∗/108 #de f i n e RED {1.0 f , 0 . 1 f , 0 . 1 f , 1 . 0 f }109 #de f i n e GREEN {0.1 f , 1 . 0 f , 0 . 1 f , 1 . 0 f }110 #de f i n e YELLOW {1.0 f , 1 . 0 f , 0 . 1 f , 1 . 0 f }111 #de f i n e BLUE {0.1 f , 0 . 1 f , 1 . 0 f , 1 . 0 f }112113 /∗ Set Misce l l aneous ∗/114 #de f i n e p i VX PI //3.1415926535115 #de f i n e ZERO MASS 0.0000001116117 /∗ Se t t i ng s o f the l i nkag e s ∗/118 #de f i n e LINK THICKNESS 0 .5119 #de f i n e SIZE DUMMY LINK 0.2∗LINK THICKNESS120 #de f i n e LINK OFFSET LINK THICKNESS121 #de f i n e NUMBER OF LINKS 4 /∗ Inc lud ing dummy l i n k s used in the DH tab l e ∗/122 #de f i n e INDEX OF LINK HELD BY USER 4123 #de f i n e SELECT SHAPE 0 /∗ f o r c i l i n d e r / sphere s e l e c t =1, otherwi se a box∗/124125 /∗ Def ine types ∗/126 typede f double trans format ionMatr ix [ 4 ] [ 4 ] ;127 typede f double vector4 [ 4 ] ;128 typede f double vector3 [ 3 ] ;129 typede f double matrix33 [ 3 ] [ 3 ] ;130131 /∗Global parameters ∗/132 // trans format ionMatr ix ∗A;133134 /∗ Def ine the f y s i c s from Vortex .∗/135 us ing namespace Vx ;136137 /∗ GLOBAL VARIABLES ===========================================================================∗/138139 /∗ Declare g l oba l v a r i a b l e s ∗/140 in t nol = NUMBER OF LINKS;141 /∗ DH parameters ∗/142 double ∗DH a , ∗DH d , ∗DH alpha , ∗DH theta , ∗DH mass , ∗DHWD;143 in t ∗DH RP;144 /∗Transformation Matr ices from frame ( i −1)−> frame ( i ) ∗/145 VxReal44 ∗A;146 VxVisua l i ze r ∗ v i s u a l i z e r ;147 VxFrame ∗ frame ;148 VxUniverse ∗ un ive r s e ;149 VxVector3 VE OFFSET Position ;150 VxVector3 VE OFFSET Orientation ;151152 /∗ CommunicationPart∗/153 ///∗ de f i n e the normal communications and c o l l i s i o n de t e c t i on func t i on s to be run in separa te threads

∗/154 // void normalComm( void ∗) ;155 // void Co l l i s i o n ( void ∗) ;156157 /∗∗/158 const char u r i [ ] = ”shmem:// foobar : 1 ” ; /∗Address Shared Memory∗/159 const t boo l ean nonblocking = f a l s e ; /∗Declare va r i ab l e ∗/160 const t i n t s e n d bu f f e r s i z e = 8∗NUM OF ELEMENTS; /∗Set send bu f f e r s i z e to 240 bytes ∗/161 const t i n t r e c e i v e b u f f e r s i z e = 8∗NUM OF ELEMENTS; /∗Set r e c e i v e bu f f e r s i z e to 240 bytes ∗/162 const char ∗ l o c a l e = NULL; /∗Declare va r i ab l e ∗/163 /∗∗/164 t u i n t numofelements = NUM OF ELEMENTS; /∗Declare va r i ab l e ∗/165166 /∗ The array o f data being r e c ev i ed from the model ∗/167 t doub le data from model [NUM OF ELEMENTS ] ;168169 /∗ The array o f data being sent to the model . ∗/

38

170 t doub le data to model [NUM OF ELEMENTS ] ;171172 /∗ The array o f data being sent to the normalComm thread from the Co l l i s i t o n thread . ∗/173 t doub le d a t a f r om c o l l i s i o n [NUM OF ELEMENTS ] ;174175 /∗ The array o f data being sent to the Co l l i s i o n thread from the normalComm thread . ∗/176 t doub le d a t a t o c o l l i s i o n [NUM OF ELEMENTS ] ;177 t st ream c l i e n t ; /∗Declare va r i ab l e ∗/178 t e r r o r r e s u l t ; /∗Declare va r i ab l e ∗/179180 char message [ 5 1 2 ] ;181 i n t running = 1 ;182 in t running comm = 1 ;183184 /∗ END DEFINE AND INCLUDE ===================================================================∗/185186 /∗187 The Mutex c l a s s i s to ac c e s s g l oba l v a r i a b l e s in d i f f e r e n t threads .188 Each time you acc e s s a va r i ab l e in a thread , you lock the mutex and once done , you unlock i t .189 Needed f o r the communication190 ∗/191 c l a s s CMutex192 {193 pr i va t e :194 CRITICAL SECTION cs ; // Windows ’ ba s i c mutex ob j e c t .195196 pub l i c :197 void Lock ( ) {198 En t e rC r i t i c a l S e c t i on (&cs ) ;199 }200201 void Unlock ( ) {202 LeaveCr i t i c a l S e c t i on (&cs ) ;203 }204205 CMutex ( ) {206 I n i t i a l i z e C r i t i c a l S e c t i o n (&cs ) ;207 }208209 ˜CMutex ( ) {210 De l e t eC r i t i c a l S e c t i o n (&cs ) ;211 }212 } ;213214 c l a s s Mechanism {215216 pub l i c :217 Mechanism ( VxVisua l i ze r ∗ v i s u a l i z e r , VxUniverse∗ i n Un ive r s e ) : mUniverse ( in Un ive r s e ) , mVisua l i zer (

v i s u a l i z e r ) {}218 v i r t u a l ˜Mechanism ( ) {}219220 void CreateBasePart ( ) ;221 void CreatePart (VxReal3 basepo s i t i on out , i n t i , VxReal3 b a s e p o s i t i o n i n ) ;// VxVisua l i ze r ∗

i n v i s u a l i z e r ) ;222 void CreateAssembly ( ) ;223 void D i s a b l eCo l l i s i o n s ( ) ;224225 /∗ de f i n e the normal communications and c o l l i s i o n de t e c t i on func t i on s to be run in separa te threads ∗/226 void normalComm( void ∗) ;227 void Co l l i s i o n ( void ∗ , double ∗mech) ;228229 VxUniverse∗ const mUniverse ; /∗ The un ive r s e where the hinge w i l l be g iven . ∗/230 VxPart∗ mBasePart ; /∗ This i s the f i x ed part . ∗/231 VxPart∗ mParts [NUMBER OF LINKS ] ;232 VxAssembly∗ mAssembly ;233 VxVisua l i ze r ∗ mVisua l i zer ;234235 /∗Add enough space f o r p o s s i b l e c on s t r a i n t s in to the mechanism ∗/236 VxHinge∗ mHinge [NUMBER OF LINKS ] ;237 VxPrismatic∗ mPrismatic [NUMBER OF LINKS ] ;238 VxBallAndSocket∗ mBallAndSocket [NUMBER OF LINKS ] ;239 VxUniversal∗ mUniversal [NUMBER OF LINKS ] ;240 VxCyl indr ica l ∗ mCyl indr ica l [NUMBER OF LINKS ] ;241 VxDoubleHinge∗ mDoubleHinge [NUMBER OF LINKS ] ;242243 } ; /∗END Class Mechanism∗/244245 void Matrix4Vector4 (VxReal4 outVector , VxReal44 ∗matrix , VxReal4 inVector )246 {247 in t i , j ;248249 f o r ( i =0; i <4; i++)250 {251 outVector [ i ] = 0 ;252 f o r ( j =0; j <4; j++)253 {254 outVector [ i ] += (∗matrix ) [ j ] [ i ]∗ inVector [ j ] ;255 }

39

256 }257 }258259 void Matrix4Matrix4 (VxReal44 ∗matrixout , VxReal44 ∗matrixin1 , VxReal44 ∗matr ix in2 )260 {261 in t i , j , k ;262 VxReal44 matrixTemp ;263264 f o r (k=0; k<4 ; k++)265 {266 f o r ( i =0; i <4; i++)267 {268 matrixTemp [ i ] [ k ] = 0 ;269270 f o r ( j =0; j <4; j++)271 {272 matrixTemp [ i ] [ k ] += ∗ (∗ (∗ ( matr ix in1 )+j )+k) ∗ ∗ (∗ (∗ ( matr ix in2 )+i )+j ) ;273 }274 }275 }276277 f o r ( i =0; i <4; i++)278 {279 f o r ( j =0; j <4; j++)280 {281 ∗ (∗ (∗ ( matrixout )+i )+j ) = matrixTemp [ i ] [ j ] ;282 }283 }284 }285286 /∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/287 /∗Mult ip ly a Matrix with a Vector∗/288 double∗ Matr ixvector ( trans format ionMatr ix ∗matrix , double ∗vector , i n t rows , i n t c o l s )289 {290 double v temp [ 4 ] , v o r i [ 4 ] ;291 double ∗ vectorout ;292 in t i , j , l ;293 vectorout = ( double ∗) mal loc ( c o l s ∗ s i z e o f ( double ) ) ;294295 f o r ( l =0; l<c o l s ; l++)296 {297 v o r i [ l ] = ∗( vec tor+l ) ;298 v temp [ l ] = 0 ;299 }300301 f o r ( i =0; i<c o l s ; i++)302 {303 f o r ( j =0; j<rows ; j++)304 {305 v temp [ i ] = v temp [ i ]+(∗matrix ) [ i ] [ j ]∗ v o r i [ j ] ;306 p r i n t f (”\n %d \ t %d \ t t e s t345 %f ” , i , j , (∗ matrix ) [ i ] [ j ] ) ;307 }308 }309310 f o r ( j =0; j<c o l s ; j++)311 {312 /∗Remember the zt and pt va lues f o r the c a l c u l a t i o n s pt temp1 = A1∗p0 , pt temp2 = A0∗pt temp1 ∗/313 v o r i [ j ] = v temp [ j ] ;314 v temp [ j ] = 0 ;315 ∗( vectorout+j ) = v o r i [ j ] ;316 }317 return vectorout ;318 f r e e ( vectorout ) ;319 }320321 /∗Mult ip ly a Matrix with a Matrix∗/322 double∗∗ MatrixMatrix ( double ∗∗matrix , double ∗∗matrix2 , i n t rows1 , i n t co l s1 , i n t c o l s 2 )323 {324 double ∗∗v temp , ∗∗ v or i , ∗∗matrixout ;325 in t i , j , k , l , m;326327 v temp = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;328 v o r i = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;329 matrixout = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;330331 f o r ( j =0; j<rows1 ; j++)332 {333 ∗( v temp+j ) = ( double ∗) mal loc ( c o l s 1 ∗ s i z e o f ( double ) ) ;334 ∗( v o r i+j ) = ( double ∗) mal loc ( c o l s 1 ∗ s i z e o f ( double ) ) ;335 ∗( matrixout+j ) = ( double ∗) mal loc ( c o l s 2 ∗ s i z e o f ( double ) ) ;336 }337338 f o r ( l =0; l<c o l s 2 ; l++)339 {340 f o r (m=0;m<rows1 ;m++)341 {342 ∗(∗( v o r i+m)+l ) = ∗(∗( matrix+m)+l ) ;343 ∗(∗( v temp+m)+l ) = 0 ;

40

344 }345 }346347 f o r ( j =0; j<rows1 ; j++)348 {349 f o r (k=0; k<c o l s 2 ; k++)//2350 {351 f o r ( i =0; i<c o l s 1 ; i++) //1352 {353 //∗(∗( v temp+j )+k) = ∗(∗( v temp+j )+k) + ∗(∗( matrix+j )+i )∗ ∗(∗( matrix2+i )+k) ;354 ∗(∗( v temp+j )+k) = ∗(∗( v temp+j )+k) + ∗(∗( matrix+j )+i )∗ ∗(∗( matrix2+i )+k) ;355 }356 }357 }358359 f o r (k=0; k<c o l s 2 ; k++)360 {361 f o r ( j =0; j<rows1 ; j++)362 {363 /∗Remember the zt and pt va lues f o r the c a l c u l a t i o n s pt temp1 = A1∗p0 , pt temp2 = A0∗pt temp1 ∗/364 //∗(∗( matrixout+j )+k) = ∗(∗( v temp+j )+k) ;365 ∗(∗( matrixout+j )+k) = ∗(∗( v temp+j )+k) ;366 }367 }368369 return matrixout ;370 f o r ( i =0; i<rows1 ; i++)371 {372 f r e e (∗ ( matrixout+i ) ) ;373 f r e e (∗ ( v temp+i ) ) ;374 f r e e (∗ ( v o r i+i ) ) ;375 }376 f r e e ( matrixout ) ;377 f r e e ( v temp ) ;378 f r e e ( v o r i ) ;379 }380381 /∗Transpose o f a Matrix∗/382 double∗∗ TransposeMatrix ( double ∗∗matrix , i n t rows , i n t c o l s )383 {384 double ∗∗Transpose ;385 in t k , l , j ;386387 Transpose = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;388 f o r ( j =0; j<c o l s ; j++)389 {390 ∗( Transpose+j ) = ( double ∗) mal loc ( rows∗ s i z e o f ( double ) ) ;391 }392393 f o r (k=0;k<rows ; k++)394 {395 f o r ( l =0; l<c o l s ; l++)396 {397 ∗(∗( Transpose+l )+k) = ∗(∗( matrix+k)+l ) ;398 }399 }400401 return Transpose ;402 in t i ;403 f o r ( i =0; i<c o l s ; i++)404 {405 f r e e (∗ ( Transpose+i ) ) ;406 }407 f r e e ( Transpose ) ;408 }409 /∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗∗/410 double∗ CreateLinkSize ( i n t i , double∗ Link S i z e )411 {412 /∗ I n i t i a l i z e the v a r i a b l e s ∗/413 double Link Radius , Link Length ;414415 Link Radius = LINK THICKNESS ;416 /∗Link Length ∗/417 Link Length = sq r t ( ( (∗ (DH a+i ) ) ∗ (∗ (DH a+i ) ) ) + ( (∗ (DH d+i ) ) ∗ (∗ (DH d+i ) ) ) ) ;418419 /∗chop o f f the o f f s e t o f f the l i n k o f f the l i n k s i z e ∗/420 Link Length = Link Length − 2∗(LINK OFFSET) ;421422 /∗Check whether or not the l i n k l e ng th becomes zero or even sma l l e r then zero , i f s o , s e t the value o f

the l i n k l e n g t h to 0 .1∗ Link Thickness ∗/423 i f ( Link Length <= 0.0001)424 {425 Link Length = 0.1∗LINK THICKNESS ;426 }427428 ∗( L ink S i z e+0) = Link Radius ;429 ∗( L ink S i z e+1) = Link Length ;430

41

431 return L ink S i z e ;432 } /∗END CalcPos i t i on s ∗/433434 void computeJacobianMassMatrix (Mechanism ∗Mech Adress , double ∗∗∗D, double ∗∗∗ jacobian , i n t rows , i n t

c o l s )435 {436 Mechanism MechanismTemp = ∗Mech Adress ;437 i n t i , j , k , l ;438 /∗ l i n k t rans fo rmat ion matr i ces ( vec tor o f dof 4X4 matr i ces ) ∗/439 vector4 ∗zt , ∗pt ;440 VxReal4 ∗p0 , ∗z0 ;441442 /∗Def ine the vector p0 b which i s the s e l e c t i n g ∗/443 double p0 b [ 4 ] = {0 , 0 , 0 , 1} ;444 /∗p0−matrix 3xn , vector with the d i s t ance between the end coord inate f rame and a frame be fo r e wr i t t en

in the baseframe = (p60−pn0 ) ∗/445 double z0 b [ 4 ] = {0 , 0 , 1 , 0} ;446 double J p [ 3 ] , Jp temp [ 3 ] , J o [ 3 ] ;447 VxReal4 p t o r i , z t o r i , p , z ;448 double PP [ 3 ] [ 3 ] ;449450 /∗ DH tab l e s t o r e s DH parameters f o r each dof row−wise ∗/451452 //A = ( trans format ionMatr ix ∗) mal loc ( dof ∗ s i z e o f ( t rans format ionMatr ix ) ) ;453 zt = ( vector4 ∗) mal loc ( nol ∗ s i z e o f ( vector4 ) ) ;454 pt = ( vector4 ∗) mal loc ( nol ∗ s i z e o f ( vector4 ) ) ;455 p0 = (VxReal4 ∗) mal loc ( ( nol+1) ∗ s i z e o f (VxReal4 ) ) ;456 z0 = (VxReal4 ∗) mal loc ( ( nol+1) ∗ s i z e o f (VxReal4 ) ) ;457458 /∗ I n s e r t the f i r s t z d i r e c t i o n o f the coord inate f rame in the ∗z0 matrix .∗/459 f o r ( j =0; j <4; j++)460 {461 (∗ z0 ) [ j ]= z0 b [ j ] ;462 (∗p0 ) [ j ]=p0 b [ j ] ;463 }464465 f o r ( i =0; i<nol ; i++) {466 /∗ bu i ld pt and zt matr i ces ∗/467 /∗ r e s e t pt and zt to begin va lues p0 b and z0 b , r e s e t pt temp and zt temp ∗/468 f o r ( j =0; j <4; j++)469 {470 /∗ s e t pt and zt to the o r i g i n a l va lues ∗/471 p t o r i [ j ] = p0 b [ j ] ;472 z t o r i [ j ] = z0 b [ j ] ;473 }474475 /∗ l i s the number o f matr i ces which the vector must be mu l t i p l i ed with : p1=A(10) ∗p0 , p2=A(21) ∗A

(10) ∗p0 etc . ∗/476 f o r ( l =0; l <( i +1) ; l++)477 {478 Matrix4Vector4 (p ,A+(i−l ) , p t o r i ) ;479 Matrix4Vector4 ( z ,A+(i−l ) , z t o r i ) ;480481 f o r ( j =0; j <4; j++)482 {483 /∗Remember the zt and pt va lues f o r the c a l c u l a t i o n s pt temp1 = A1∗p0 , pt temp2 = A0∗

pt temp1 ∗/484 z t o r i [ j ] = z [ j ] ;485 p t o r i [ j ] = p [ j ] ;486 }487488 }489490 f o r ( j =0; j <4; j++)491 {492 /∗Save pt temp in the p0 matrix∗/493 (∗p0+4∗( i +1) ) [ j ]= p t o r i [ j ] ;494 (∗ z0+4∗( i +1) ) [ j ]= z t o r i [ j ] ;495 }496497 } /∗end f o r loop nol ∗/498499 /∗Build Jacobian from p0 and z∗/500 in t i i ;501 f o r ( i i =0; i i <nol ; i i ++) {502 f o r ( i =0; i<nol ; i++) {503 i f (DH RP[ i ]==1) /∗Revolute j o i n t ∗/504 {505 /∗ d i s tance between endpoint ( po int l ength (DHRP) ) and point n−1∗/506 f o r ( j =0; j <3; j++)507 {508 p [ j ] = ∗(∗( p0+i i )+j ) + ∗(DHWD+i i )∗ ( ∗(∗( p0+( i i +1) )+j )−∗(∗(p0+i i )+j ) ) − ∗(∗(

p0+i )+j ) ;509 }510511 /∗Calcu la te the jacob ian in two part s ∗/512 PP [ 0 ] [ 0 ] = 0 ;513 PP [ 0 ] [ 1 ] = p [ 2 ] ;

42

514 PP [ 0 ] [ 2 ] = −p [ 1 ] ;515516 PP [ 1 ] [ 0 ] = −p [ 2 ] ;517 PP [ 1 ] [ 1 ] = 0 ;518 PP [ 1 ] [ 2 ] = p [ 0 ] ;519520 PP [ 2 ] [ 0 ] = p [ 1 ] ;521 PP [ 2 ] [ 1 ] = −p [ 0 ] ;522 PP [ 2 ] [ 2 ] = 0 ;523524 f o r ( j =0; j <3; j++)525 {526 Jp temp [ j ]=0;527 }528 f o r ( j =0; j <3; j++)529 {530 f o r (k=0; k<3; k++)531 {532 /∗ bu i ld the Jp vector ∗/533 Jp temp [ j ] = Jp temp [ j ]+PP[ j ] [ k ]∗ (∗ z0+4∗ i ) [ k ] ;534 }535 f o r ( l =0; l <3; l++)536 {537 /∗Save Jp temp in the Jp vector ∗/538 J p [ l ]=Jp temp [ l ] ;539 }540 }541542 f o r ( j =0; j <3; j++)543 {544 J o [ j ] = (∗ z0+4∗ i ) [ j ] ;545 }546547 f o r ( j =0; j <3; j++)548 {549 ∗ (∗ (∗ ( jacob ian+i i )+i )+j ) = J p [ j ] ;550 ∗ (∗ (∗ ( jacob ian+i i )+i )+j +3) = J o [ j ] ;551 }552 }553 e l s e i f (DH RP[ i ]==0) /∗Prismat ic j o i n t ∗/554 {555556 f o r ( j =0; j <3; j++)557 {558 J p [ j ] = (∗ z0+4∗ i ) [ j ] ;559 J o [ j ] = 0 ;560 }561562 f o r ( j =0; j <3; j++)563 {564 ∗ (∗ (∗ ( jacob ian+i i )+i )+j ) = J p [ j ] ;565 ∗ (∗ (∗ ( jacob ian+i i )+i )+j +3) = J o [ j ] ;566 }567 }568 }569570 i f ( i i <(nol ) )571 {572 f o r ( j =0; j <6; j++)573 {574 f o r ( i=i i +1; i<nol ; i++)575 {576 ∗ (∗ (∗ ( jacob ian+i i )+i )+j ) = 0 ;577 }578 }579 }580 }581582 /∗Dea l l oca te memory∗/583 f r e e ( zt ) ;584 f r e e ( pt ) ;585 f r e e ( p0 ) ;586 f r e e ( z0 ) ;587 /∗END Jacobian Ca l cu l a t i on s ∗/588589 /∗BEGIN MasMatrix Ca l cu l a t i on s ∗/590 /∗Al l o ca t ing memory f o r the MassMatrix Ca l cu l a t i on s ∗/591 in t r , m, n ;592 VxReal33 I n e r t i aLo ca l ; // , In e r t i aAbso lu t e ;593 double ∗∗∗D temp , ∗∗∗ I I ;594 double ∗∗R, ∗∗A n ;595596 double ∗∗∗TransposedJacobian upper ;597 double ∗∗∗TransposedJacobian upper t imesmass ;598 double ∗∗∗TransposedJacobian lower ;599 double ∗∗∗ Jacobian upper ;600 double ∗∗∗ Jacob ian lower ;601

43

602 TransposedJacobian upper = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;603 TransposedJacobian upper t imesmass = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;604 TransposedJacobian lower = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;605 D temp = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;606 A n = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;607 R = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;608 f o r ( i =0; i<nol ; i++)609 {610 ∗( TransposedJacobian upper t imesmass+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;611 ∗( TransposedJacobian lower+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;612 ∗( TransposedJacobian upper+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;613 ∗(D temp+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;614 ∗(A n+i ) = ( double ∗) mal loc (4∗ s i z e o f ( double ) ) ;615 ∗(R+i ) = ( double ∗) mal loc (3∗ s i z e o f ( double ) ) ;616 f o r ( r=0; r<nol ; r++)617 {618 ∗(∗( TransposedJacobian upper t imesmass+i )+r ) = ( double ∗) mal loc (3∗ s i z e o f ( double ) ) ;619 ∗(∗( TransposedJacobian lower+i )+r ) = ( double ∗) mal loc (3∗ s i z e o f ( double ) ) ;620 ∗(∗( TransposedJacobian upper+i )+r ) = ( double ∗) mal loc (3∗ s i z e o f ( double ) ) ;621 ∗(∗(D temp+i )+r ) = ( double ∗) mal loc (3∗ s i z e o f ( double ) ) ;622 }623 }624625 Jacobian upper = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;626 Jacob ian lower = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;627 I I = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;628629 f o r ( i =0; i<nol ; i++)630 {631 ∗( Jacobian upper+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;632 ∗( Jacob ian lower+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;633 ∗( I I+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;634635 f o r ( r=0; r <3; r++)636 {637 ∗(∗( Jacobian upper+i )+r ) = ( double ∗) mal loc ( nol∗ s i z e o f ( double ) ) ;638 ∗(∗( Jacob ian lower+i )+r ) = ( double ∗) mal loc ( nol∗ s i z e o f ( double ) ) ;639 ∗(∗( I I+i )+r ) = ( double ∗) mal loc (3∗ s i z e o f ( double ) ) ;640 }641 }642643 /∗ I n i t i a l i z e Matr ices ∗/644 f o r ( i =0; i<nol ; i++)645 {646 /∗Set R to eye−matrix∗/647 f o r (k=0;k<3;k++)648 {649 f o r ( l =0; l <3; l++)650 {651 /∗ I n i t i a l i z e the i n e r t i a matrix to zero ∗/652 ∗ (∗ (∗ ( I I+i )+k)+l ) = 0 ;653 /∗ I n i t i a l i z e the RotationMatrix to zero ∗/654 ∗(∗(R+k)+l ) = 0 ;655 }656 }657 /∗ I n i t i a l i z e the RotationMatrix to iden t i tymat r i x ∗/658 ∗(∗(R+0)+0) = 1 ;659 ∗(∗(R+1)+1) = 1 ;660 ∗(∗(R+2)+2) = 1 ;661 }662663 /∗ Star t c a l c u l a t i o n s ∗/664665 f o r ( i =0; i<nol ; i++)666 {667 /∗Determine the Ine r t i aTenso r o f the l i n k in l o c a l coo rd ina t e s ∗/668 MechanismTemp . mParts [ i ]−>g e t I n e r t i a ( I n e r t i aLo ca l ) ;669 //MechanismTemp . mParts [ i ]−>ge t In e r t i aAbso lu t e ( Ine r t i aAbso lu t e ) ;670671 /∗Set the A n matrix to the new A matrix (Ai ) ∗/672 f o r (m=0;m<3;m++)673 {674 f o r (n=0;n<3;n++)675 {676 ∗(∗(A n+m)+n) = ∗ (∗ (∗ (A+i )+m)+n) ;677 }678 }679 /∗Calcu la te the new Rotat iona l matrix us ing the old R and the new A n (Rn = R∗A n) ∗/680 R = MatrixMatrix (R, A n , 3 , 3 , 3 ) ;681682 /∗Because Jacobian i s de f ined Transposed i t should be transposed to get the normal∗/683 f o r ( l =0; l <3; l++)684 {685 f o r (k=0;k<nol ; k++)686 {687 ∗ (∗ (∗ ( Jacob ian lower+i )+l )+k) = ∗ (∗ (∗ ( jacob ian+i )+k)+l +3) ;688 ∗ (∗ (∗ ( Jacobian upper+i )+l )+k) = ∗ (∗ (∗ ( jacob ian+i )+k)+l ) ;689 }

44

690 }691692 /∗ convert the type VxReal33 to po in t e r ∗/693 f o r ( l =0; l <3; l++)694 {695 f o r (k=0;k<3;k++)696 {697 ∗ (∗ (∗ ( I I+i )+l )+k) = In e r t i aLo c a l [ l ] [ k ] ;698 }699 }700701 ∗( TransposedJacobian upper+i ) = TransposeMatrix (∗ ( Jacobian upper+i ) ,3 , no l ) ;702 ∗( TransposedJacobian lower+i ) = TransposeMatrix (∗ ( Jacob ian lower+i ) ,3 , nol ) ;703 /∗Calcu la te the transposed upperpart o f the Jacobian ∗ mass ∗/704 f o r ( l =0; l <3; l++)705 {706 f o r (k=0;k<nol ; k++)707 {708 (∗ (∗ (∗ ( TransposedJacobian upper t imesmass+i )+k)+l ) ) = ∗(DH mass+i )∗ (∗ (∗ (∗ (

TransposedJacobian upper+i )+k)+l ) ) ;709 }710 }711 /∗ [ 1 ] Ca l cu la te m∗J ’∗ J ∗/712 ∗(D temp+i ) = MatrixMatrix (∗ ( TransposedJacobian upper t imesmass+i ) ,∗ ( Jacobian upper+i ) , nol , 3 ,

no l ) ;713 /∗ [ 2 ] Ca l cu la te J∗R∗ I ∗R’∗ J ∗/714 ∗(D+i ) = MatrixMatrix ( MatrixMatrix ( MatrixMatrix ( MatrixMatrix (∗ ( TransposedJacobian lower+

i ) ,R, nol , 3 , 3 ) ,∗ ( I I+i ) , nol , 3 , 3 ) , TransposeMatrix (R, 3 , 3 ) , nol , 3 , 3 ) ,∗ ( Jacob ian lower+i ) , nol , 3 , nol) ;

715716 /∗add the two part s ( [ 1 ] + [ 2 ] ) toge the r ∗/717 f o r (n=0;n<nol ; n++)718 {719 f o r (m=0;m<nol ;m++)720 {721 ∗ (∗ (∗ (D+i )+m)+n) = ∗ (∗ (∗ (D+i )+m)+n) + ∗ (∗ (∗ (D temp+i )+m)+n) ;722 }723 }724725 /∗add the s epa ra t e l y ( per l i n k ) c a l cu l a t ed MassMatrices∗/726 i f ( i >0)727 {728 f o r (n=0;n<nol ; n++)729 {730 f o r (m=0;m<nol ;m++)731 {732 ∗ (∗ (∗ (D+i )+m)+n) = ∗ (∗ (∗ (D+i −1)+m)+n) + ∗ (∗ (∗ (D+i )+m)+n) ;733 }734 }735 }736 }737738 /∗Dea l l o ca t i on o f memory∗/739 f o r ( i =0; i <3; i++)740 {741 f o r ( r=0; r<nol ; r++)742 {743 f r e e (∗ (∗ ( TransposedJacobian upper+i )+r ) ) ;744 f r e e (∗ (∗ ( TransposedJacobian upper t imesmass+i )+r ) ) ;745 f r e e (∗ (∗ ( TransposedJacobian lower+i )+r ) ) ;746 }747 f r e e (∗ (A n+i ) ) ;748 f r e e (∗ (R+i ) ) ;749 }750 f o r ( i =0; i<nol ; i++)751 {752 f o r ( r=0; r <3; r++)753 {754 f r e e (∗ (∗ ( Jacobian upper+i )+r ) ) ;755 f r e e (∗ (∗ ( Jacob ian lower+i )+r ) ) ;756 f r e e (∗ (∗ ( I I+i )+r ) ) ;757 }758 f o r ( r=0; r<nol ; r++)759 {760 f r e e (∗ (∗ (D temp+i )+r ) ) ;761 }762 }763 } /∗End MassMatrix∗/764765 void computeTransMatrix (VxReal44 ∗A, double ∗DH a , double ∗DH d , double ∗DH alpha , double ∗DH theta , i n t

∗DH RP, in t nol )766 {767 in t i ;768 /∗Build t rans fo rmat ion matr i ces ∗/769 f o r ( i =0; i<nol ; i++) {770 ∗ (∗ (∗ (A+i )+0)+0) = cos (∗ ( DH theta+i ) ) ;771 ∗ (∗ (∗ (A+i )+1)+0) = −s i n (∗ ( DH theta+i ) ) ∗ cos (∗ ( DH alpha+i ) ) ;772 ∗ (∗ (∗ (A+i )+2)+0) = s in (∗ ( DH theta+i ) ) ∗ s i n (∗ ( DH alpha+i ) ) ;

45

773 ∗ (∗ (∗ (A+i )+3)+0) = ∗(DH a+i ) ∗ cos (∗ ( DH theta+i ) ) ;774775 ∗ (∗ (∗ (A+i )+0)+1) = s in (∗ ( DH theta+i ) ) ;776 ∗ (∗ (∗ (A+i )+1)+1) = cos (∗ ( DH theta+i ) ) ∗ cos (∗ ( DH alpha+i ) ) ;777 ∗ (∗ (∗ (A+i )+2)+1) = −cos (∗ ( DH theta+i ) ) ∗ s i n (∗ ( DH alpha+i ) ) ;778 ∗ (∗ (∗ (A+i )+3)+1) = ∗(DH a+i ) ∗ s i n (∗ ( DH theta+i ) ) ;779780 ∗ (∗ (∗ (A+i )+0)+2) = 0 . 0 ;781 ∗ (∗ (∗ (A+i )+1)+2) = s in (∗ ( DH alpha+i ) ) ;782 ∗ (∗ (∗ (A+i )+2)+2) = cos (∗ ( DH alpha+i ) ) ;783 ∗ (∗ (∗ (A+i )+3)+2) = ∗(DH d+i ) ;784785 ∗ (∗ (∗ (A+i )+0)+3) = 0 . 0 ;786 ∗ (∗ (∗ (A+i )+1)+3) = 0 . 0 ;787 ∗ (∗ (∗ (A+i )+2)+3) = 0 . 0 ;788 ∗ (∗ (∗ (A+i )+3)+3) = 1 . 0 ;789 }790 } /∗END BUILD A MATRICES∗/791792 void Mechanism : : CreateAssembly ( )793 {794 /∗Make a new assembly which we can add to the un ive r s e when f i l l e d with part s . This i s needed

because we don ’ t know yet how many l i n k s we w i l l have∗/795 mAssembly = new VxAssembly ;796 mUniverse −> addAssembly (mAssembly ) ;797 }798799 void Mechanism : : CreateBasePart ( )800 {801 /∗BASEPART: :BASESPHERE∗/802 f l o a t co lo rYe l low [ 4 ] = YELLOW;803 double BaseRadius ;804 VxReal44 transformMatrix ;805 VxNode BaseNode ;806807 /∗Set rad ius o f the basesphere ∗/808 BaseRadius = 0.5∗LINK THICKNESS ;809810 /∗Standard BasePart∗/811 mBasePart = new VxPart ;812 mBasePart −>setName (” Base Part ”) ;813814 /∗ f o r a sphere s e l e c t =1, otherwi se a box∗/815 i f (SELECT SHAPE == 1)816 {817 // sphere818 mBasePart −>addGeometry (new VxSphere ( BaseRadius ) , 0) ;819 /∗Add the base sphere to the v i s u a l i z e r ∗/820 BaseNode = mVisual izer−>createSphere ( BaseRadius , co lorYel low , 0) ;821 }822 e l s e //box823 {824 mBasePart −>addGeometry (new VxBox( BaseRadius , BaseRadius , BaseRadius ) , 0) ;825 /∗Add the base sphere to the v i s u a l i z e r ∗/826 BaseNode = mVisual izer−>createBox ( BaseRadius , BaseRadius , BaseRadius , co lorYel low , 0) ;827 }828 mBasePart −>setNode (BaseNode ) ;829830 /∗Set trans format ionMatr ix f o r the BasePart∗/831 transformMatrix [ 0 ] [ 0 ] = POS BASE X 1 ;832 transformMatrix [ 0 ] [ 1 ] = POS BASE X 2 ;833 transformMatrix [ 0 ] [ 2 ] = POS BASE X 3 ;834 transformMatrix [ 0 ] [ 3 ] = 0 ;835836 transformMatrix [ 1 ] [ 0 ] = POS BASE Y 1 ;837 transformMatrix [ 1 ] [ 1 ] = POS BASE Y 2 ;838 transformMatrix [ 1 ] [ 2 ] = POS BASE Y 3 ;839 transformMatrix [ 1 ] [ 3 ] = 0 ;840841 transformMatrix [ 2 ] [ 0 ] = POS BASE Z 1 ;842 transformMatrix [ 2 ] [ 1 ] = POS BASE Z 2 ;843 transformMatrix [ 2 ] [ 2 ] = POS BASE Z 3 ;844 transformMatrix [ 2 ] [ 3 ] = 0 ;845846 transformMatrix [ 3 ] [ 0 ] = POS BASE X ;847 transformMatrix [ 3 ] [ 1 ] = POS BASE Y ;848 transformMatrix [ 3 ] [ 2 ] = POS BASE Z ;849 transformMatrix [ 3 ] [ 3 ] = 1 ;850851 mBasePart −>setTransform ( transformMatrix ) ;852 mBasePart −>updateNode ( ) ;853854 /∗Set basepart as not dynamical∗/855 mBasePart −>f r e e z e ( t rue ) ;856 mBasePart −>s e tCont ro l ( VxEntity : : kControlDynamic ) ;857 mBasePart −>setMass (1000) ;858 /∗Add basepart to the Universe ∗/859 mUniverse −>addEntity (mBasePart ) ;

46

860861 /∗END BASEPART: :BASESPHERE∗/862 }863864 void Mechanism : : CreatePart (VxReal3 bas epos i t i on out , i n t i , VxReal3 b a s e p o s i t i o n i n )865 {866 f l o a t c o l o r [ 4 ] ;867 VxReal4 pos i , pos i min 1 , pos M , o r i e n t a t i o n i ;868 VxReal4 Se l e c t Pos = {0 , 0 , 0 , 1} ;869 VxReal4 Se l e c t Z Ax i s = {0 , 0 , 1 , 0} ;870 VxReal44 c o l o r v e c t o r = {RED, GREEN, BLUE, YELLOW} ;871 VxReal3 ba s epo s i t i on = {POS BASE X,POS BASE Y,POS BASE Z} ;872 double ∗Link S i z e ;873 VxVector3 u1 , u2 , u3 , P;874 VxNode Node ;875 in t n , j , l , k ;876 VxReal44 transformMatrix , Base Transform ;877 VxReal44 ∗ transmatr ix ,∗ transmatr ix2 ,∗ t ransmatr ix3 ;878 L ink S i z e = ( double ∗) mal loc (2∗ s i z e o f ( double ) ) ;879 transmatr ix = (VxReal44 ∗) mal loc ( s i z e o f (VxReal44 ) ) ;880 transmatr ix2 = (VxReal44 ∗) mal loc ( s i z e o f (VxReal44 ) ) ;881 transmatr ix3 = (VxReal44 ∗) mal loc ( s i z e o f (VxReal44 ) ) ;882883 /∗ Determine o r i e n t a t i o n and po s i t i o n o f the new part ∗/884 /∗ START POSITION of l i n k i ∗/885 /∗ From base to f i r s t frame 1∗/886 f o r ( j =0; j <3; j++)887 {888 pos i m in 1 [ j ] = ba s e p o s i t i o n i n [ j ] ;889 }890 pos i m in 1 [ 3 ] = 1 ;891892 /∗ s e l e c t the t rans fo rmat ion matrix o f the base part ∗/893 mBasePart−>getTransform ( Base Transform ) ;894895 /∗ I n i t i a l i s e t ransmatr ix to Base Transform∗/896 f o r ( j =0; j <4; j++)897 {898 f o r ( l =0; l <4; l++)899 {900 ∗ (∗ (∗ ( t ransmatr ix )+j )+l ) = Base Transform [ j ] [ l ] ;901 }902 }903904 /∗Calcu la te po s i t i o n o f frame i−1∗/905 f o r (n=0;n<( i ) ; n++)906 {907 Matrix4Matrix4 ( transmatr ix , transmatr ix ,A+n) ; /∗ from frame 0 up to frame i−1∗/908 }909 Matrix4Vector4 ( pos i min 1 , transmatr ix , Se l e c t Pos ) ;910911 /∗ I n i t i a l i s e transmatr ix2 to A( frame i −1)−>( f r ame i ) ∗/912 f o r ( j =0; j <4; j++)913 {914 f o r ( l =0; l <4; l++)915 {916 ∗ (∗ (∗ ( t ransmatr ix2 )+j )+l ) = ∗ (∗ (∗ (A+i )+j )+l ) ;917 }918 }919 /∗Calcu la te po s i t i o n o f frame i ∗/920 Matrix4Matrix4 ( transmatr ix2 , transmatr ix , t ransmatr ix2 ) ; /∗ from frame i up to frame i +1∗/921 Matrix4Vector4 ( pos i , transmatr ix2 , Se l e c t Pos ) ;922923 /∗Set ba s epo s i t i on ou t to be pos i , which w i l l become the ba s e p o s i t i o n i n o f the next part ∗/924 f o r ( l =0; l <3; l++)925 {926 ba s epo s i t i on ou t [ l ] = po s i [ l ] ;927 }928929 /∗Now po s i ( p o s i t i o n o f frame i ) and pos i m in 1 ( po s i t i o n o f frame i −1) are known vec to r s in VxReal4

∗/930931 /∗Trans late the frame po s i t i o n s to the l i n k po s i t i o n932 pos M i s the po s i t i o n o f the frame o f l i n k i ∗/933 f o r ( j =0; j <4; j++)934 {935 pos M [ j ] = (0 . 5∗ ( po s i m in 1 [ j ] + po s i [ j ] ) ) ;936 }937938 /∗ ORIENTATION of l i n k i ∗/939 /∗ Determine z−ax i s d i r e c t i o n o f l i n k i ∗/940 i f (∗ (DH a+i ) !=0 | | ∗(DH d+i ) !=0)941 {942 f o r ( j =0; j <3; j++)943 {944 u3 [ j ] = ( po s i [ j ]− pos i m in 1 [ j ] ) ;945 }946 }

47

947 e l s e /∗ I f c on s t r a i n t i s gonna be at the same po s i t i o n as the l a s t c on s t r a i n t ∗/948 {949 /∗ I n i t i a l i s e transmatr ix3 to Base Transform∗/950 f o r ( j =0; j <4; j++)951 {952 f o r ( l =0; l <4; l++)953 {954 ∗ (∗ (∗ ( t ransmatr ix3 )+j )+l ) = Base Transform [ j ] [ l ] ;955 }956 }957 /∗Calcu la te po s i t i o n o f frame i−1∗/958 f o r (n=0;n<( i −1) ; n++)959 {960 Matrix4Matrix4 ( transmatr ix3 , transmatr ix3 ,A+i ) ; /∗ from frame 0 up to frame i−1∗/961 }962 Matrix4Vector4 ( pos i min 1 , transmatr ix3 , Se l e c t Pos ) ;963964 /∗u3 ( ax i s along the l i n k ) i s the d i r e c t i o n o f the l a s t l i n k when DH a and DH d are zero ∗/965 i f ( p o s i [0]== pos i m in 1 [ 0 ] && po s i [1]== pos i m in 1 [ 1 ] && po s i [2]== pos i m in 1 [ 2 ] )966 {967 f o r ( j =0; j <3; j++)968 {969 u3 [ j ] = po s i [ j ] ;970 }971 }972 e l s e /∗ otherwi se u3 i s in the d i r e c t i o n o f f ram i min 1−>f r ame i ∗/973 {974 f o r ( j =0; j <3; j++)975 {976 u3 [ j ] = ( po s i [ j ]− pos i m in 1 [ j ] ) ;977 }978 }979 }980981 /∗ normal ize the d i r e c t i o nv e c t o r u3∗/982 u3 . normal ize ( ) ;983984 /∗ Build a vector pe rpend i cu la r to u3 Using the plane formula : ax + by + cz − d = 0.∗/985 /∗ Because dev id ing by 0 in P i s impos s ib l e we have to check the va lues o f u3 .∗/986 i f ( ( u3 [ 2 ] ∗ u3 [ 2 ] ) <=0.0001)987 {988 i f ( ( u3 [ 1 ] ∗ u3 [ 1 ] ) <=0.0001)989 {990 P [ 1 ] = 1 ;991 P [ 2 ] = u3 [ 2 ]+1 ; /∗ to be sure that not the same point i s s e l e c t e d ∗/992 P [ 0 ] = ( ( u3 [ 0 ] ∗ u3 [ 0 ] ) + (u3 [ 1 ] ∗ u3 [ 1 ] ) + (u3 [ 2 ] ∗ u3 [ 2 ] ) − ( u3 [ 2 ] ∗ P [ 2 ] ) − ( u3 [ 1 ] ∗

P [ 1 ] ) ) /( u3 [ 0 ] ) ;993 }994 e l s e995 {996 P [ 0 ] = u3 [ 0 ]+1 ;997 P [ 2 ] = 1 ;998 P [ 1 ] = ( ( u3 [ 0 ] ∗ u3 [ 0 ] ) + (u3 [ 1 ] ∗ u3 [ 1 ] ) + (u3 [ 2 ] ∗ u3 [ 2 ] ) − ( u3 [ 0 ] ∗ P [ 0 ] ) − ( u3 [ 2 ] ∗

P [ 2 ] ) ) /( u3 [ 1 ] ) ;999 }

1000 }1001 e l s e1002 {1003 P [ 0 ] = u3 [ 0 ]+1 ;1004 P [ 1 ] = 1 ;1005 P [ 2 ] = ( ( u3 [ 0 ] ∗ u3 [ 0 ] ) + (u3 [ 1 ] ∗ u3 [ 1 ] ) + (u3 [ 2 ] ∗ u3 [ 2 ] ) − ( u3 [ 0 ] ∗ P [ 0 ] ) − ( u3 [ 1 ] ∗ P

[ 1 ] ) ) /( u3 [ 2 ] ) ;1006 }10071008 f o r ( j =0; j <3; j++)1009 {1010 u2 [ j ] = (P[ j ]−u3 [ j ] ) ;1011 }10121013 /∗Making a un i tve c to r o f the u2 vector ∗/1014 u2 . normal ize ( ) ;10151016 /∗Calcu la te the u1 d i r e c t i o n vector pe rpend i cu la r to u2 and u3 . ( u2 x u3 ) ∗/1017 /∗Be sure that the vec to r s make righthanded frame∗/1018 u1 . s e tCros s (u2 , u3 ) ;10191020 /∗ Determining the l i n k s i z e ∗/1021 L ink S i z e = CreateLinkSize ( i , L ink S i z e ) ;10221023 /∗ Set t ing the va lues o f the transformMatrix based on the d i r e c t i o n v e c t o r s u1 , u2 and u3 . u3 i s the

d i r e c t i o n from frame 1 to frame 2 , u2 and u3 are pe rpend i cu la r to eachother spanning arighthanded frame ∗/

1024 transformMatrix [ 0 ] [ 0 ] = u1 [ 0 ] ;1025 transformMatrix [ 0 ] [ 1 ] = u1 [ 1 ] ;1026 transformMatrix [ 0 ] [ 2 ] = u1 [ 2 ] ;1027 transformMatrix [ 0 ] [ 3 ] = 0 ;10281029 transformMatrix [ 1 ] [ 0 ] = u2 [ 0 ] ;

48

1030 transformMatrix [ 1 ] [ 1 ] = u2 [ 1 ] ;1031 transformMatrix [ 1 ] [ 2 ] = u2 [ 2 ] ;1032 transformMatrix [ 1 ] [ 3 ] = 0 ;10331034 transformMatrix [ 2 ] [ 0 ] = u3 [ 0 ] ;1035 transformMatrix [ 2 ] [ 1 ] = u3 [ 1 ] ;1036 transformMatrix [ 2 ] [ 2 ] = u3 [ 2 ] ;1037 transformMatrix [ 2 ] [ 3 ] = 0 ;10381039 transformMatrix [ 3 ] [ 0 ] = pos M [ 0 ] ;1040 transformMatrix [ 3 ] [ 1 ] = pos M [ 1 ] ;1041 transformMatrix [ 3 ] [ 2 ] = pos M [ 2 ] ;1042 transformMatrix [ 3 ] [ 3 ] = 1 ;10431044 /∗ I f DH a and DH d are zero then a dummy part i s made ( a smal l sphere b a s i c l y ) ∗/1045 i f (∗ (DH a+i )==0 && ∗(DH d+i )==0)1046 {1047 p r i n t f (”A l i n k o f l ength zero i s detected ( l i n k %d) , a very smal l l i n k i s generated . The

c on s t r a i n t s are generated .\n” , i +1) ;1048 i f ( i==(INDEX OF LINK HELD BY USER−1) )1049 {1050 /∗ Se l e c t a new co l o r f o r the held l i n k ∗/1051 k=0;// red1052 p r i n t f (”ERROR: You have s e l e c t e d a dummylink ( l i n k %d) to be held .\n” , i +1) ;1053 }1054 e l s e1055 {1056 /∗ Se l e c t a new co l o r f o r a dummy l i n k ∗/1057 k=2;// blue1058 }1059 f o r ( l =0; l <4; l++)1060 {1061 co l o r [ l ] = co l o r v e c t o r [ k ] [ l ] ;1062 }1063 mParts [ i ] = new VxPart ;1064 mParts [ i ] −> setCombinedName (”ZERO A D” ,”” , ( i +1) ) ;1065 // geometry i s a smal l sphere :1066 mParts [ i ] −> addGeometry (new VxSphere (SIZE DUMMY LINK) ,0) ;10671068 /∗Create part in v i s u a l i z e r & c r ea t e node to attach v i s u a l i z e r with the s imu la t ion ∗/1069 Node = mVisual izer−>createSphere (SIZE DUMMY LINK, co lor , 0) ;1070 }1071 e l s e /∗normal l i n k ( so with a length ) ∗/1072 {1073 /∗The l i n k held by user i s s e t to be red ∗/1074 i f ( i==(INDEX OF LINK HELD BY USER−1) )1075 {1076 /∗ Se l e c t a new co l o r f o r the held l i n k ∗/1077 k=0;// red1078 }1079 e l s e1080 {1081 /∗ Se l e c t a new co l o r f o r a normal l i n k ∗/1082 k=1;// green1083 }1084 /∗ f i l l the c o l o r vec tor ∗/1085 f o r ( l =0; l <4; l++)1086 {1087 co l o r [ l ] = co l o r v e c t o r [ k ] [ l ] ;1088 }10891090 /∗Build the s epe ra t e part o f a l l in format ion gathered be fo r e ∗/1091 mParts [ i ] = new VxPart ;1092 mParts [ i ] −> setCombinedName (” l i n k ” ,”” , ( i +1) ) ;10931094 // Se l e c t c y l i nd e r or box geometry f o r the l i n k s1095 /∗ f o r a cy l i nd e r s e l e c t =1, otherwi se a box∗/1096 i f (SELECT SHAPE == 1)1097 {1098 // cy l i nd e r :1099 mParts [ i ] −> addGeometry (new VxCylinder (∗ ( L ink S i z e+0) ,∗ ( L ink S i z e+1) ) ,0) ;1100 /∗Create part in v i s u a l i z e r & c r ea t e node to attach v i s u a l i z e r with the s imu la t ion ∗/1101 Node = mVisual izer−>c r ea t eCy l inde r (∗ ( L ink S i z e+0) ,∗ ( L ink S i z e+1) , co lor , 0 ) ;1102 }1103 e l s e1104 {1105 //box1106 mParts [ i ] −> addGeometry (new VxBox(∗ ( L ink S i z e+0) ,∗ ( L ink S i z e+0) ,∗ ( L ink S i z e+1) ) ,0) ;1107 /∗Create part in v i s u a l i z e r & c r ea t e node to attach v i s u a l i z e r with the s imu la t ion ∗/1108 Node = mVisual izer−>createBox (∗ ( L ink S i z e+0) ,∗ ( L ink S i z e+0) ,∗ ( L ink S i z e+1) , co lor , 0 )

;1109 }1110 }11111112 /∗Connect the v i s u a l i z e r with the part in the v i s u a l i z e r ∗/1113 mParts [ i ] −> setNode (Node ) ;1114 /∗Set the trans format ionmatr ix in mEntityTM in the mPart∗/1115 mParts [ i ] −> setTransform ( transformMatrix ) ;

49

1116 mParts [ i ] −> updateNode ( ) ;11171118 /∗The helded l i n k has to be c on t r o l l e d animated∗/1119 i f ( i==(INDEX OF LINK HELD BY USER−1) )1120 {1121 //mParts [ i ] −> s e tContro l ( VxEntity : : kControlAnimated ) ;1122 mParts [ i ] −> s e tContro l ( VxEntity : : kControlDynamic ) ;1123 }1124 e l s e1125 {1126 mParts [ i ] −> s e tContro l ( VxEntity : : kControlDynamic ) ;1127 }1128 /∗Set the mass o f the l i n k ∗/1129 mParts [ i ] −> setMass (∗ (DH mass+i ) ) ;11301131 /∗Add the new part to the assembly∗/1132 mAssembly −> addEntity (mParts [ i ] ) ;11331134 /∗ Determine o r i e n t a t i o n o f the new con s t r a i n t ∗/1135 /∗ ORIENTATION of con s t r a in t f r ame ∗/1136 /∗ s e t c on s t r a i n t z−ax i s ∗/1137 Matrix4Vector4 ( o r i e n t a t i o n i , transmatr ix , S e l e c t Z Ax i s ) ;1138 const VxReal3 c o n s t r a i n t a x i s = { o r i e n t a t i o n i [ 0 ] , o r i e n t a t i o n i [ 1 ] , o r i e n t a t i o n i [ 2 ] } ;11391140 /∗ Build the ac tua l c on s t a i n t s ∗/1141 /∗ mParts [ i−1]==mBasePart because t h i s i s b a s i c l y a po in t e r to a memory adress , so because the

mParts are added in the r i gh t order mParts [0−1] == memoryadress o f the basepart ∗/1142 i f (∗ (DH RP+i )==1) /∗Revolute ∗/1143 {1144 mHinge [ i ] = new VxHinge (mParts [ i −1] ,mParts [ i ] , b a s epo s i t i on i n , c o n s t r a i n t a x i s ) ;1145 mHinge [ i ] −>setCombinedName (” Revolute ” ,”” , i ) ;11461147 /∗Add con s t r a i n t to the assembly∗/1148 mAssembly −>addConstraint (mHinge [ i ] ) ;1149 }1150 e l s e /∗Prismat ic ∗/1151 {1152 mPrismatic [ i ] = new VxPrismatic (mParts [ i −1] , mParts [ i ] , b a s epo s i t i on i n , c o n s t r a i n t a x i s ) ;1153 mPrismatic [ i ] −>setCombinedName (” Pr i smat ic ” ,”” , i ) ;11541155 /∗Add con s t r a i n t to the assembly∗/1156 mAssembly −>addConstraint ( mPrismatic [ i ] ) ;1157 }11581159 /∗De−Al loca t e Memory∗/1160 f r e e ( L ink S i z e ) ;1161 f r e e ( t ransmatr ix ) ;1162 f r e e ( transmatr ix2 ) ;1163 f r e e ( transmatr ix3 ) ;1164 }11651166 void Mechanism : : D i s a b l eCo l l i s i o n s ( )1167 {1168 in t i ;1169 /∗Disab le c o l l i s i o n s with the BasePart∗/1170 mUniverse−>disab lePa i r sWith (mBasePart ) ;11711172 /∗Disab le c o l l i s i o n s between the new dummy part and the two part s attached to the midpoint o f the

dummy part ∗/1173 f o r ( i =0; i<NUMBER OF LINKS; i++)1174 {1175 i f (∗ (DH a+i )==0 && ∗(DH d+i )==0)1176 {1177 mUniverse−>disab lePa i r sWith (mParts [ i ] ) ;1178 }1179 }1180 }11811182 /∗ communication∗/1183 void normalComm( void ∗arg )1184 {1185 r e s u l t = stream connect ( ur i , nonblocking , s e nd bu f f e r s i z e , r e c e i v e b u f f e r s i z e , &c l i e n t ) ;1186 i f ( r e s u l t == 0)1187 {11881189 whi le ( running )1190 {11911192 r e s u l t = s t r e am re c e i v e doub l e a r r ay ( c l i e n t , &data from model [ 0 ] , numofelements ) ;1193 i f ( r e s u l t <= 0)1194 break ;11951196 /∗ Copy the whole data in data from model array to d a t a t o c o l l i s i o n array .∗/1197 memcpy(& d a t a t o c o l l i s i o n [ 0 ] , &data from model [ 0 ] , s i z e o f ( data from model ) ) ;11981199 /∗ Copy the whole data in the d a t a f r om c o l l i s i o n array to data to model array . ∗/1200 memcpy(&data to model [ 0 ] , &d a t a f r om c o l l i s i o n [ 0 ] , s i z e o f ( d a t a f r om c o l l i s i o n ) ) ;1201

50

1202 r e s u l t = st ream send doub le ar ray ( c l i e n t , &data to model [ 0 ] , numofelements ) ;1203 i f ( r e s u l t <= 0)1204 break ;12051206 r e s u l t = s t r eam f lu sh ( c l i e n t ) ;1207 }1208 } /∗END CONNECTION WITH URI∗/1209 e l s e1210 { /∗CONNECTION WITH URI FAILED∗/1211 msg get error messageA ( l o ca l e , r e su l t , message , s i z e o f ( message ) ) ;1212 p r i n t f (”\n Unable to connect to URI ’%s ’ . %s\n” , ur i , message ) ;12131214 running comm = 0 ;1215 } /∗END CONNECTION WITH URI FAILED∗/1216 s t r eam c l o s e ( c l i e n t ) ;1217 }12181219 void Co l l i s i o n ( void ∗arg , Mechanism ∗amechanism )1220 {1221 in t j = 0 ;1222 Mechanism MechanismTemp = ∗amechanism ;12231224 VxVector3 Force From Held Link ;1225 VxVector3 Pos it ion From Held Link ;1226 VxVector3 Orientat ion From Held Link ;1227 VxVector3 Ve loc i ty Linear From Held Link ;1228 VxVector3 Veloc ity Angular From Held Link ;1229 VxVector3 Of f s e t Pos i t i on From Hapt i c Dev i ce ;1230 VxVector3 Of f s e t Or i entat ion From Hapt i c Dev i ce ;1231 VxVector3 Pos i t ion From Hapt ic Device ;1232 VxVector3 Orientat ion From Hapt ic Device ;1233 VxVector3 Ve loc i ty L inear From Hapt i c Dev ice ;1234 VxVector3 Veloc i ty Angular From Hapt ic Device ;1235 VxVector3 Force Vector , Force Vector P , Force Vector D ;1236 VxVector3 Torque Vector , Torque Vector P , Torque Vector D ;1237 /∗ s e t the o f f s e t v e c t o r to the va lues o f the s t a r t p o s i t i o n o f the held l i n k ∗/1238 MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−>ge tPo s i t i on (VE OFFSET Position ) ;1239 MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−>getOrientationEulerXYZ (VE OFFSET Orientation ) ;124012411242 whi le ( running && running comm )1243 {1244 /∗ s tepp ing the s imula t i on and the v i s u a l i z a t i o n ∗/1245 frame−>s tep ( ) ;1246 running = v i s u a l i z e r−>update ( ) ;12471248 /∗ Set the d a t a f r om c o l l i s i o n array to j . ∗/1249 /∗Get the p o s t i t i o n / r o t a t i on / l i n e a r v e l o c i t y / angu l a r v e l o c i t y / f o r c e from the l i nkage ∗/1250 MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−>ge tPo s i t i on ( Pos it ion From Held Link ) ;1251 MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−>getOrientationEulerXYZ (

Orientat ion From Held Link ) ;1252 MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−>getForce ( Force From Held Link ) ;1253 MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−>ge tL inea rVe l o c i ty (

Ve loc i ty Linear From Held Link ) ;1254 MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−>getAngularVe loc i ty (

Veloc i ty Angular From Held Link ) ;12551256 Pos i t ion From Hapt ic Device [ 0 ] = d a t a t o c o l l i s i o n [ 0 ] ;1257 Pos i t ion From Hapt ic Device [ 1 ] = d a t a t o c o l l i s i o n [ 1 ] ;1258 Pos i t ion From Hapt ic Device [ 2 ] = d a t a t o c o l l i s i o n [ 2 ] ;1259 Orientat ion From Hapt ic Device [ 0 ] = d a t a t o c o l l i s i o n [ 3 ] ;1260 Orientat ion From Hapt ic Device [ 1 ] = d a t a t o c o l l i s i o n [ 4 ] ;1261 Orientat ion From Hapt ic Device [ 2 ] = d a t a t o c o l l i s i o n [ 5 ] ;1262 Ve loc i ty L inear From Hapt i c Dev ice [ 0 ] = d a t a t o c o l l i s i o n [ 6 ] ;1263 Ve loc i ty L inear From Hapt i c Dev ice [ 1 ] = d a t a t o c o l l i s i o n [ 7 ] ;1264 Ve loc i ty L inear From Hapt i c Dev ice [ 2 ] = d a t a t o c o l l i s i o n [ 8 ] ;1265 Veloc i ty Angular From Hapt ic Device [ 0 ] = d a t a t o c o l l i s i o n [ 9 ] ;1266 Veloc i ty Angular From Hapt ic Device [ 1 ] = d a t a t o c o l l i s i o n [ 1 0 ] ;1267 Veloc i ty Angular From Hapt ic Device [ 2 ] = d a t a t o c o l l i s i o n [ 1 1 ] ;12681269 /∗ Sca l e the obtained po s i t i o n s to be in SI ∗/1270 Pos i t ion From Hapt ic Device . s c a l e (LENGTH SCALE ON GRAPHICS) ;12711272 Of f s e t Pos i t i on From Hapt i c Dev i ce . setSum ( Pos it ion From Haptic Device , VE OFFSET Position ) ;1273 Of f s e t Or i entat ion From Hapt i c Dev i ce . setSum ( Orientat ion From Haptic Device ,

VE OFFSET Orientation ) ;12741275 Force Vector P . s e tD i f f e r e n c e ( Of f s e t Pos i t i on From Hapt i c Dev ice , Pos i t ion From Held Link ) ;1276 Force Vector P . s c a l e (P LINEAR) ;12771278 Force Vector D . s e tD i f f e r e n c e ( Ve loc i ty Linear From Hapt ic Device , Ve loc i ty Linear From Held Link )

;1279 Force Vector D . s c a l e (D LINEAR) ;12801281 Torque Vector P . s e tD i f f e r e n c e ( Of f se t Or ientat ion From Hapt i c Dev ice , Orientat ion From Held Link )

;1282 //Torque Vector P . s e tD i f f e r e n c e ( Orientat ion From Haptic Device , Orientat ion From Held Link ) ;1283 Torque Vector P . s c a l e (P ROTATION) ;

51

12841285 Torque Vector D . s e tD i f f e r e n c e ( Veloc i ty Angular From Haptic Device ,

Veloc i ty Angular From Held Link ) ;1286 Torque Vector D . s c a l e (D ROTATION) ;12871288 Force Vector . setSum ( Force Vector P , Force Vector D ) ;1289 Torque Vector . setSum ( Torque Vector P , Torque Vector D ) ;12901291 p r i n t f (”\n Forces Applied : x %f ; y %f ; z%f ” , Force Vector [ 0 ] , Force Vector [ 1 ] , Force Vector [ 2 ] ) ;1292 p r i n t f (”\n Torques Applied : x %f ; y %f ; z%f ” , Torque Vector [ 0 ] , Torque Vector [ 1 ] , Torque Vector

[ 2 ] ) ;1293 /∗Add the f o r c e from the s imul ink f i l e to the l i nkage ∗/1294 //MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−> s e tPo s i t i o n ( AppliedForce ) ;1295 MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−>addForce ( Force Vector ) ;1296 MechanismTemp . mParts [ INDEX OF LINK HELD BY USER−1]−>addTorque ( Torque Vector ) ;12971298 d a t a f r om c o l l i s i o n [ 0 ] = Force Vector [ 0 ] ;1299 d a t a f r om c o l l i s i o n [ 1 ] = Force Vector [ 1 ] ;1300 d a t a f r om c o l l i s i o n [ 2 ] = Force Vector [ 2 ] ;1301 d a t a f r om c o l l i s i o n [ 3 ] = Torque Vector [ 3 ] ;1302 d a t a f r om c o l l i s i o n [ 4 ] = Torque Vector [ 4 ] ;1303 d a t a f r om c o l l i s i o n [ 5 ] = Torque Vector [ 5 ] ;1304 d a t a f r om c o l l i s i o n [ 6 ] = Of f s e t Pos i t i on From Hapt i c Dev i ce [ 0 ] ;1305 d a t a f r om c o l l i s i o n [ 7 ] = Of f s e t Pos i t i on From Hapt i c Dev i ce [ 1 ] ;1306 d a t a f r om c o l l i s i o n [ 8 ] = Of f s e t Pos i t i on From Hapt i c Dev i ce [ 2 ] ;1307 d a t a f r om c o l l i s i o n [ 9 ] = Pos it ion From Held Link [ 0 ] ;1308 d a t a f r om c o l l i s i o n [ 1 0 ] = Pos it ion From Held Link [ 1 ] ;1309 d a t a f r om c o l l i s i o n [ 1 1 ] = Pos it ion From Held Link [ 2 ] ;1310 d a t a f r om c o l l i s i o n [ 1 2 ] = d a t a t o c o l l i s i o n [ 1 2 ] ;1311 d a t a f r om c o l l i s i o n [ 1 3 ] = d a t a t o c o l l i s i o n [ 1 3 ] ;1312 d a t a f r om c o l l i s i o n [ 1 4 ] = d a t a t o c o l l i s i o n [ 1 4 ] ;1313 d a t a f r om c o l l i s i o n [ 1 5 ] = d a t a t o c o l l i s i o n [ 1 5 ] ;1314 d a t a f r om c o l l i s i o n [ 1 6 ] = d a t a t o c o l l i s i o n [ 1 6 ] ;1315 d a t a f r om c o l l i s i o n [ 1 7 ] = d a t a t o c o l l i s i o n [ 1 7 ] ;1316 d a t a f r om c o l l i s i o n [ 1 8 ] = d a t a t o c o l l i s i o n [ 1 8 ] ;1317 d a t a f r om c o l l i s i o n [ 1 9 ] = d a t a t o c o l l i s i o n [ 1 9 ] ;1318 d a t a f r om c o l l i s i o n [ 2 0 ] = d a t a t o c o l l i s i o n [ 2 0 ] ;1319 d a t a f r om c o l l i s i o n [ 2 1 ] = d a t a t o c o l l i s i o n [ 2 1 ] ;1320 d a t a f r om c o l l i s i o n [ 2 2 ] = d a t a t o c o l l i s i o n [ 2 2 ] ;1321 d a t a f r om c o l l i s i o n [ 2 3 ] = d a t a t o c o l l i s i o n [ 2 3 ] ;1322 d a t a f r om c o l l i s i o n [ 2 4 ] = d a t a t o c o l l i s i o n [ 2 4 ] ;1323 d a t a f r om c o l l i s i o n [ 2 5 ] = d a t a t o c o l l i s i o n [ 2 5 ] ;1324 d a t a f r om c o l l i s i o n [ 2 6 ] = d a t a t o c o l l i s i o n [ 2 6 ] ;1325 d a t a f r om c o l l i s i o n [ 2 7 ] = d a t a t o c o l l i s i o n [ 2 7 ] ;1326 d a t a f r om c o l l i s i o n [ 2 8 ] = d a t a t o c o l l i s i o n [ 2 8 ] ;1327 d a t a f r om c o l l i s i o n [ 2 9 ] = d a t a t o c o l l i s i o n [ 2 9 ] ;13281329 j += 1 ;13301331 }1332 /∗Shut down the v i s u a l i z e r ∗/1333 v i s u a l i z e r−>stopViewer ( ) ;1334 frame−>r e l e a s e ( ) ;1335 d e l e t e v i s u a l i z e r ;1336 running = 0 ;1337 }13381339 VxUniverse∗ CreateUniverse (VxFrame∗ frame )1340 {1341 /∗Make un ive r s e ∗/1342 VxUniverse∗ un ive r s e = new VxUniverse ( ) ;1343 /∗Add the un ive r s e to the inputted VxFrame∗/1344 frame−>addUniverse ( un ive r s e ) ;1345 /∗Set g rav i ty in d i r e c t i o n (x , y , z ) ) ∗/1346 universe−>se tGrav i ty (0 , 0 , 0) ;1347 // universe−>d i s ab l eA l lPa i r s ( ) ;1348 universe−>con f i gu r e ( ) ;13491350 return un ive r s e ;1351 } /∗END CREATEUNIVERSE∗/13521353 in t main ( i n t argc , const char ∗argv [ ] )1354 {1355 /∗BEGIN DECLARE COMMUNICATION∗/135613571358 /∗ I n i t i a l i z i n g a l l the ar rays . ∗/1359 unsigned in t n o e ;1360 f o r ( n o e = 0 ; n o e < numofelements ; n o e++)1361 {1362 data from model [ n o e ] = 0 ;1363 data to model [ n o e ] = 0 ;1364 d a t a f r om c o l l i s i o n [ n o e ] = 0 ;1365 d a t a t o c o l l i s i o n [ n o e ] = 0 ;1366 }1367 /∗END DECLARE COMMUNICATION∗/13681369 /∗BEGIN DECLARE VISUALISATION AND SIMULATION∗/

52

1370 in t i , j , r , rows = 6 , c o l s=NUMBER OF LINKS;1371 /∗ Jacobian matrix 6xn∗/1372 /∗ ∗∗∗ Jacobian & ∗∗∗D= 3D Matrix = s t ru c tu r e in Matlab∗/1373 double ∗∗∗ jacobian , ∗∗∗D;1374 VxReal3 ba s epo s i t i on ou t = {0 ,0 ,0} ;1375 VxReal3 b a s e p o s i t i o n i n = {POS BASE X,POS BASE Y,POS BASE Z} ;1376 /∗ memory a l l o c a t i o n ∗/1377 DH a = ( double ∗) mal loc ( nol ∗ s i z e o f ( double ) ) ;1378 DH d = ( double ∗) mal loc ( nol ∗ s i z e o f ( double ) ) ;1379 DH alpha = ( double ∗) mal loc ( nol ∗ s i z e o f ( double ) ) ;1380 DH theta = ( double ∗) mal loc ( nol ∗ s i z e o f ( double ) ) ;1381 DH RP = ( in t ∗) mal loc ( nol ∗ s i z e o f ( i n t ) ) ;1382 DH mass = ( double ∗) mal loc ( nol ∗ s i z e o f ( double ) ) ;1383 DHWD = ( double ∗) mal loc ( nol ∗ s i z e o f ( double ) ) ;1384 A = (VxReal44 ∗) mal loc ( ( nol ) ∗ s i z e o f (VxReal44 ) ) ;13851386 jacob ian = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;1387 f o r ( i =0; i <6; i++)1388 {1389 ∗( jacob ian+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;1390 f o r ( r=0; r<NUMBER OF LINKS; r++)1391 {1392 ∗(∗( jacob ian+i )+r ) = ( double ∗) mal loc (6∗ s i z e o f ( double ) ) ;1393 }1394 }13951396 D = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;1397 f o r ( i =0; i<NUMBER OF LINKS; i++)1398 {1399 ∗(D+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;1400 f o r ( r=0; r<NUMBER OF LINKS; r++)1401 {1402 ∗(∗(D+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;1403 }1404 }14051406 /∗ i n i t i a l i z a t i o n s ∗/1407 /∗ Put l eng ths e tc in CM∗/1408 ∗(DH a+0) = 5 . 0 ; ∗(DH a+1) = 0 . 0 ; ∗(DH a+2) = 0 . 0 ; ∗(DH a+3)

= 0 . 0 ;1409 ∗(DH d+0) = 2 . 0 ; ∗(DH d+1) = 0 . 0 ; ∗(DH d+2) = 0 . 0 ; ∗(DH d+3)

= 5 . 0 ;1410 ∗(DH alpha+0) = 0.5∗ pi ; ∗(DH alpha+1) = −0.5∗ pi ; ∗(DH alpha+2) = 0∗0.5∗ pi ; ∗(DH alpha+3)

= 0.5∗ pi ;1411 ∗(DH theta+0) = 0∗ pi /3 ; ∗(DH theta+1) = 0.0∗ pi ; ∗(DH theta+2) = 0.5∗ pi ; ∗(DH theta+3)

= 0 ;1412 ∗(DH RP+0) = 1 ; ∗(DH RP+1) = 1 ; ∗(DH RP+2) = 1 ; ∗(DH RP+3)

= 0 ;1413 ∗(DH mass+0) = 0 . 1 ; ∗(DH mass+1) = ZERO MASS;∗ ( DH mass+2) = ZERO MASS;∗ ( DH mass+3)

= 0 . 2 ;1414 ∗(DHWD+0) = 0 . 5 ; ∗(DHWD+1) = 0 . 5 ; ∗(DHWD+2) = 0 . 5 ; ∗(DHWD+3)

= 0 . 5 ;14151416 /∗Compute the t rans fo rmat ionMatr i ce s and the po s i t i o n s o f the l i n k s ∗/1417 computeTransMatrix (A, DH a , DH d , DH alpha , DH theta , DH RP, nol ) ;1418 /∗END DECLARE VISUALISATION AND SIMULATION∗/14191420 /∗END DECLARE AND SET∗/14211422 /∗Create frame wherein everyth ing i s happening/ ca l cu l a t ed ∗/1423 frame = VxFrame : : i n s tance ( ) ;1424 VxFrameReg i s te rAl l Interact ions ( frame ) ;14251426 /∗Create the par t s in the un ive r s e ∗/1427 v i s u a l i z e r = VxVisua l i ze r : : c r e a t e ( ) ;1428 /∗Use a standard window f o r the v i s u a l i z a t i o n ∗/1429 v i s u a l i z e r−>createDefaultWindow ( argc , argv , ”Mechanism”) ;14301431 /∗ I n t e g r a t e t imestep ∗/1432 const VxReal timeStep = .001 f ; / / . 001 f ;1433 /∗Set the t imestep to the frame ∗/1434 frame−>setTimeStep ( timeStep ) ;1435 v i s u a l i z e r−>setFrameRateLock ( t rue ) ;14361437 /∗Create the un ive r s e from th i s frame∗/1438 un ive r s e = CreateUniverse ( frame ) ;1439 /∗ I n s e r t the un ive r s e to the v i s u a l i z e r ∗/1440 v i s u a l i z e r−>s e tUn ive r s e ( un ive r s e ) ;14411442 /∗Create an in s tance o f Mechanism ∗/1443 Mechanism aMechanism ( v i s u a l i z e r , un ive r s e ) ;1444 aMechanism . CreateAssembly ( ) ;1445 aMechanism . CreateBasePart ( ) ;1446 f o r ( i =0; i<NUMBER OF LINKS; i++)1447 {1448 aMechanism . CreatePart ( ba s epos i t i on out , i , b a s e p o s i t i o n i n ) ;1449 f o r ( j =0; j <3; j++)1450 {

53

1451 ba s e p o s i t i o n i n [ j ] = ba s epo s i t i on ou t [ j ] ;1452 }1453 }1454 aMechanism . D i s a b l eCo l l i s i o n s ( ) ;1455 p r i n t f (”End I n i t i a l i z i n g the Linkage \n”) ;1456 Mechanism ∗Mech Adress = &aMechanism ;14571458 /∗Calcu la te the MassMatrix and Jacobian ∗/1459 computeJacobianMassMatrix ( Mech Adress , D, jacobian , rows , c o l s ) ;14601461 /∗Open and s e t opt ions f o r / in v i s u a l i z e r ∗/1462 v i s u a l i z e r−>s tar tViewer ( ) ;1463 v i s u a l i z e r−>setCameraView (60 ,VX DEG2RAD(−90) ,0) ;1464 v i s u a l i z e r−>createHe lp ( ) ;1465 v i s u a l i z e r−>saveState ( ) ;1466 v i s u a l i z e r−>setFrameRateLock (0) ;1467 v i s u a l i z e r−>setDisplayTime (1) ;1468 const char ∗Escape = ”Use [ESC] ape to e x i t ” ;1469 v i s u a l i z e r−>createTextDisp lay (10 ,10 , Escape , 0 ) ;14701471 /∗ I n i t i a l i z e the v i s u a l i z e r and the s imu lat i on ∗/1472 running = v i s u a l i z e r−>update ( ) ;1473 frame−>s tep ( ) ;14741475 /∗ s t a r t s epe ra t e thread f o r the communication∗/1476 // void ∗ s t a r t t h r e ad1 ;1477 beg inthread (normalComm , 0 , NULL) ;1478 Sleep (5000) ; /∗Wait 5 seconds to g ive the program enough time to i n i t i a l i z e ∗/1479 Co l l i s i o n (NULL, Mech Adress ) ;14801481 /∗ i f running i s 1 don ’ t continue , running i s s e t in the Co l l i s i o n thread when you shut the

v i s u a l i z e r down running !=0∗/1482 whi le ( running ) ;14831484 /∗De−a l l o c a t e memory∗/1485 f r e e (DH a) ;1486 f r e e (DH d) ;1487 f r e e (DH alpha ) ;1488 f r e e ( DH theta ) ;1489 f r e e (DH RP) ;1490 f r e e (DH mass ) ;1491 f r e e (A) ;1492 f r e e (DHWD) ;14931494 /∗ check t h i s ∗/1495 f o r ( i =0; i <6; i++)1496 {1497 f o r ( r=0; r<nol ; r++)1498 {1499 f r e e (∗ (∗ ( jacob ian+i )+r ) ) ;1500 }1501 // f r e e (∗ ( jacob ian+i ) ) ;1502 }1503 // f r e e ( jacob ian ) ;1504 f o r ( i =0; i<nol ; i++)1505 {1506 f o r ( r=0; r<nol ; r++)1507 {1508 f r e e (∗ (∗ (D+i )+r ) ) ;1509 }1510 }1511 /∗END Dea l l oca te memory∗/15121513 p r i n t f (”\n Press ente r to e x i t ”) ;1514 getchar ( ) ;15151516 return 0 ;1517 } /∗END main∗/

54

E Source code c-file Mass Matrix12 /∗3 C−f unc t i on : MassMatrix . cpp45 Desc r ip t i on : This f i l e c a l c u l a t e s the MassMatrix6 Around l i n e 500 s e t the l i n k s i z e s78 Authors : Erik Hijkoop ( e . g . hi jkoop@student . tue . n l )9

10 Inputs :1112 Outputs :1314 Date :1516 ∗/17 /∗ Inc lude VX part s ∗/18 #inc lude ”Vx/VxPrec is ion . h”19 #inc lude ”Vx/VxFrame . h”20 #inc lude ”Vx/VxBox . h”21 #inc lude ”Vx/VxColl is ionGeometry . h”22 #inc lude ”Vx/VxPart . h”23 #inc lude ”Vx/VxHinge . h”24 #inc lude ”Vx/VxPrismatic . h”25 #inc lude ”Vx/VxUniverse . h”26 #inc lude ”Vx/ VxConstra intContro l l e r . h”27 #inc lude ”Vx/ VxVisua l i ze r . h”28 #inc lude ”Vx/VxSpring . h”29 #inc lude ”Vx/VxVector3 . h”30 #inc lude ”Vx/VxBox . h”31 #inc lude ”Vx/VxTransform . h”32 #inc lude ”Vx/VxEventSubscriber . h”33 #inc lude ”Vx/VxPlane . h”34 #inc lude ”Vx/VxContactPropert ies . h”35 #inc lude ”Vx/VxPlane . h”36 #inc lude ”Vx/VxConvexMesh . h”37 #inc lude ”Vx/VxContactPropert ies . h”38 #inc lude ”Vx/VxRequest . h”3940 /∗ Inc lude standard ∗/41 #inc lude ” s tda fx . h”42 #inc lude ”windows . h”43 #inc lude ” s td i o . h”44 #inc lude ” proce s s . h”45 #inc lude ”math . h”46 #inc lude ” ctime”47 #inc lude ” crtdbg . h”484950 /∗Types de f inen ∗/51 typede f double trans format ionMatr ix [ 4 ] [ 4 ] ;52 typede f double vector4 [ 4 ] ;53 typede f double vector3 [ 3 ] ;54 typede f double matrix33 [ 3 ] [ 3 ] ;5556 /∗ Star t d e f i n i n g parameters ∗/57 #de f i n e p i 3.1415926535859 /∗Global parameters ∗/60 trans format ionMatr ix ∗A;6162 /∗Functions de f inen ∗/6364 /∗Mult ip ly a Matrix with a Vector∗/65 double∗ Matr ixvector ( trans format ionMatr ix ∗matrix , double ∗vector , i n t rows , i n t c o l s )66 {67 double v temp [ 4 ] , v o r i [ 4 ] ;68 double ∗ vectorout ;69 i n t i , j , l ;70 vectorout = ( double ∗) mal loc ( c o l s ∗ s i z e o f ( double ) ) ;7172 f o r ( l =0; l<c o l s ; l++)73 {74 v o r i [ l ] = ∗( vec tor+l ) ;75 v temp [ l ] = 0 ;76 }7778 f o r ( i =0; i<c o l s ; i++)79 {80 f o r ( j =0; j<rows ; j++)81 {82 v temp [ i ] = v temp [ i ]+(∗matrix ) [ i ] [ j ]∗ v o r i [ j ] ;83 p r i n t f (”\n %d \ t %d \ t t e s t345 %f ” , i , j , (∗ matrix ) [ i ] [ j ] ) ;84 }85 }

55

8687 f o r ( j =0; j<c o l s ; j++)88 {89 /∗Remember the zt and pt va lues f o r the c a l c u l a t i o n s pt temp1 = A1∗p0 , pt temp2 = A0∗pt temp1 ∗/90 v o r i [ j ] = v temp [ j ] ;91 v temp [ j ] = 0 ;92 ∗( vectorout+j ) = v o r i [ j ] ;93 }94 return vectorout ;95 f r e e ( vectorout ) ;96 }9798 /∗Mult ip ly a Matrix with a Matrix∗/99 double∗∗ MatrixMatrix ( double ∗∗matrix , double ∗∗matrix2 , i n t rows1 , i n t co l s1 , i n t c o l s 2 )

100 {101 double ∗∗v temp , ∗∗ v or i , ∗∗matrixout ;102 in t i , j , k , l , m;103104 v temp = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;105 v o r i = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;106 matrixout = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;107108 f o r ( j =0; j<rows1 ; j++)109 {110 ∗( v temp+j ) = ( double ∗) mal loc ( c o l s 1 ∗ s i z e o f ( double ) ) ;111 ∗( v o r i+j ) = ( double ∗) mal loc ( c o l s 1 ∗ s i z e o f ( double ) ) ;112 ∗( matrixout+j ) = ( double ∗) mal loc ( c o l s 2 ∗ s i z e o f ( double ) ) ;113 }114115 f o r ( l =0; l<c o l s 2 ; l++)116 {117 f o r (m=0;m<rows1 ;m++)118 {119 ∗(∗( v o r i+m)+l ) = ∗(∗( matrix+m)+l ) ;120 ∗(∗( v temp+m)+l ) = 0 ;121 }122 }123124 f o r ( j =0; j<rows1 ; j++)125 {126 f o r (k=0; k<c o l s 2 ; k++)//2127 {128 f o r ( i =0; i<c o l s 1 ; i++) //1129 {130 //∗(∗( v temp+j )+k) = ∗(∗( v temp+j )+k) + ∗(∗( matrix+j )+i )∗ ∗(∗( matrix2+i )+k) ;131 ∗(∗( v temp+j )+k) = ∗(∗( v temp+j )+k) + ∗(∗( matrix+j )+i )∗ ∗(∗( matrix2+i )+k) ;132 }133 }134 }135136 f o r (k=0; k<c o l s 2 ; k++)137 {138 f o r ( j =0; j<rows1 ; j++)139 {140 /∗Remember the zt and pt va lues f o r the c a l c u l a t i o n s pt temp1 = A1∗p0 , pt temp2 = A0∗pt temp1 ∗/141 //∗(∗( matrixout+j )+k) = ∗(∗( v temp+j )+k) ;142 ∗(∗( matrixout+j )+k) = ∗(∗( v temp+j )+k) ;143 }144 }145146 return matrixout ;147 f o r ( i =0; i<rows1 ; i++)148 {149 f r e e (∗ ( matrixout+i ) ) ;150 f r e e (∗ ( v temp+i ) ) ;151 f r e e (∗ ( v o r i+i ) ) ;152 }153 f r e e ( matrixout ) ;154 f r e e ( v temp ) ;155 f r e e ( v o r i ) ;156 }157158 /∗Transpose o f a Matrix∗/159 double∗∗ TransposeMatrix ( double ∗∗matrix , i n t rows , i n t c o l s )160 {161 double ∗∗Transpose ;162 in t k , l , j ;163164 Transpose = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;165 f o r ( j =0; j<c o l s ; j++)166 {167 ∗( Transpose+j ) = ( double ∗) mal loc ( rows∗ s i z e o f ( double ) ) ;168 }169170 f o r (k=0;k<rows ; k++)171 {172 f o r ( l =0; l<c o l s ; l++)173 {

56

174 ∗(∗( Transpose+l )+k) = ∗(∗( matrix+k)+l ) ;175 }176 }177178 return Transpose ;179 in t i ;180 f o r ( i =0; i<c o l s ; i++)181 {182 f r e e (∗ ( Transpose+i ) ) ;183 }184 f r e e ( Transpose ) ;185 }186187 void computeMassMatrix ( double ∗∗∗D, double ∗∗∗ jacobian , i n t rows , i n t co l s ,188 double ∗DH a , double ∗DH d , double ∗DH alpha , double ∗DH theta , i n t ∗DH RP, double

∗DH WD, double∗DH mass ,189 i n t dof )190 {191 in t i , j , k , l ;192 /∗ l i n k t rans fo rmat ion matr i ces ( vec tor o f dof 4X4 matr i ces ) ∗/193 vector4 ∗zt , ∗pt , ∗p0 , ∗z0 ;194195 /∗Def ine the vector p0 b which i s the s e l e c t i n g ∗/196 double p0 b [ 4 ] = {0 , 0 , 0 , 1} ;197 /∗p0−matrix 3xn , vector with the d i s t ance between the end coord inate f rame and a frame be fo r e wr i t t en

in the baseframe = (p60−pn0 ) ∗/198 double z0 b [ 4 ] = {0 , 0 , 1 , 0} ;199 double z t o r i [ 4 ] , p t o r i [ 4 ] , p [ 3 ] , J p [ 3 ] , Jp temp [ 3 ] , J o [ 3 ] ;200 double PP [ 3 ] [ 3 ] ;201202 /∗ DH tab l e s t o r e s DH parameters f o r each dof row−wise ∗/203204 //A = ( trans format ionMatr ix ∗) mal loc ( dof ∗ s i z e o f ( t rans format ionMatr ix ) ) ;205 zt = ( vector4 ∗) mal loc ( dof ∗ s i z e o f ( vector4 ) ) ;206 pt = ( vector4 ∗) mal loc ( dof ∗ s i z e o f ( vector4 ) ) ;207 p0 = ( vector4 ∗) mal loc ( ( dof+1) ∗ s i z e o f ( vector4 ) ) ;208 z0 = ( vector4 ∗) mal loc ( ( dof+1) ∗ s i z e o f ( vector4 ) ) ;209210211 /∗Build t rans fo rmat ion matr i ces ∗/212 f o r ( i =0; i<dof ; i++) {213214 (∗A+4∗ i ) [ 0 ] [ 0 ] = cos (∗ ( DH theta+i ) ) ;215 (∗A+4∗ i ) [ 0 ] [ 1 ] = −s i n (∗ ( DH theta+i ) ) ∗ cos (∗ ( DH alpha+i ) ) ;216 (∗A+4∗ i ) [ 0 ] [ 2 ] = s in (∗ ( DH theta+i ) ) ∗ s i n (∗ ( DH alpha+i ) ) ;217 (∗A+4∗ i ) [ 0 ] [ 3 ] = ∗(DH a+i ) ∗ cos (∗ ( DH theta+i ) ) ;218219 (∗A+4∗ i ) [ 1 ] [ 0 ] = s in (∗ ( DH theta+i ) ) ;220 (∗A+4∗ i ) [ 1 ] [ 1 ] = cos (∗ ( DH theta+i ) ) ∗ cos (∗ ( DH alpha+i ) ) ;221 (∗A+4∗ i ) [ 1 ] [ 2 ] = −cos (∗ ( DH theta+i ) ) ∗ s i n (∗ ( DH alpha+i ) ) ;222 (∗A+4∗ i ) [ 1 ] [ 3 ] = ∗(DH a+i ) ∗ s i n (∗ ( DH theta+i ) ) ;223224 (∗A+4∗ i ) [ 2 ] [ 0 ] = 0 . 0 ;225 (∗A+4∗ i ) [ 2 ] [ 1 ] = s in (∗ ( DH alpha+i ) ) ;226 (∗A+4∗ i ) [ 2 ] [ 2 ] = cos (∗ ( DH alpha+i ) ) ;227 (∗A+4∗ i ) [ 2 ] [ 3 ] = ∗(DH d+i ) ;228229 (∗A+4∗ i ) [ 3 ] [ 0 ] = 0 . 0 ;230 (∗A+4∗ i ) [ 3 ] [ 1 ] = 0 . 0 ;231 (∗A+4∗ i ) [ 3 ] [ 2 ] = 0 . 0 ;232 (∗A+4∗ i ) [ 3 ] [ 3 ] = 1 . 0 ;233 } /∗END BUILD A MATRICES∗/234235 /∗ I n s e r t the f i r s t z d i r e c t i o n o f the coord inate f rame in the ∗z0 matrix .∗/236 f o r ( j =0; j <4; j++)237 {238 (∗ z0 ) [ j ]= z0 b [ j ] ;239 (∗p0 ) [ j ]=p0 b [ j ] ;240 }241242 f o r ( i =0; i<dof ; i++) {243 /∗ bu i ld pt and zt matr i ces ∗/244 /∗ r e s e t pt and zt to begin va lues p0 b and z0 b , r e s e t pt temp and zt temp ∗/245 f o r ( j =0; j <4; j++)246 {247 /∗ s e t pt and zt to the o r i g i n a l va lues ∗/248 p t o r i [ j ] = p0 b [ j ] ;249 z t o r i [ j ] = z0 b [ j ] ;250 }251252 /∗ l i s the number o f matr i ces which the vector must be mu l t i p l i ed with : p1=A(10) ∗p0 , p2=A(21) ∗A

(10) ∗p0 etc . ∗/253 f o r ( l =0; l <( i +1) ; l++)254 {255256 double ∗p , ∗z ;257 p=Matr ixvector ( (A+(i−l ) ) , p t o r i , 4 , 4 ) ;258 z=Matr ixvector ( (A+(i−l ) ) , z t o r i , 4 , 4 ) ;

57

259260 f o r ( j =0; j <4; j++)261 {262 /∗Remember the zt and pt va lues f o r the c a l c u l a t i o n s pt temp1 = A1∗p0 , pt temp2 = A0∗

pt temp1 ∗/263 z t o r i [ j ] = ∗( z+j ) ;264 p t o r i [ j ] = ∗(p+j ) ;265 }266267 }268269 f o r ( j =0; j <4; j++)270 {271 /∗Save pt temp in the p0 matrix∗/272 (∗p0+4∗( i +1) ) [ j ]= p t o r i [ j ] ;273 (∗ z0+4∗( i +1) ) [ j ]= z t o r i [ j ] ;274 }275276 } /∗end f o r loop dof ∗/277278 /∗Build Jacobian from p0 and z∗/279 in t i i ;280 f o r ( i i =0; i i <dof ; i i ++) {281 f o r ( i =0; i<dof ; i++) {282 i f (DH RP[ i ]==1) /∗Revolute j o i n t ∗/283 {284 /∗ d i s tance between endpoint ( po int l ength (DHRP) ) and point n−1∗/285 f o r ( j =0; j <3; j++)286 {287 p [ j ] = ∗(∗( p0+i i )+j ) + ∗(DHWD+i i )∗ ( ∗(∗( p0+( i i +1) )+j )−∗(∗(p0+i i )+j ) ) − ∗(∗(

p0+i )+j ) ;288 }289290 /∗Calcu la te the jacob ian in two part s ∗/291 PP [ 0 ] [ 0 ] = 0 ;292 PP [ 0 ] [ 1 ] = p [ 2 ] ;293 PP [ 0 ] [ 2 ] = −p [ 1 ] ;294295 PP [ 1 ] [ 0 ] = −p [ 2 ] ;296 PP [ 1 ] [ 1 ] = 0 ;297 PP [ 1 ] [ 2 ] = p [ 0 ] ;298299 PP [ 2 ] [ 0 ] = p [ 1 ] ;300 PP [ 2 ] [ 1 ] = −p [ 0 ] ;301 PP [ 2 ] [ 2 ] = 0 ;302303 f o r ( j =0; j <3; j++)304 {305 Jp temp [ j ]=0;306 }307 f o r ( j =0; j <3; j++)308 {309 f o r (k=0; k<3; k++)310 {311 /∗ bu i ld the Jp vector ∗/312 Jp temp [ j ] = Jp temp [ j ]+PP[ j ] [ k ]∗ (∗ z0+4∗ i ) [ k ] ;313 }314 f o r ( l =0; l <3; l++)315 {316 /∗Save Jp temp in the Jp vector ∗/317 J p [ l ]=Jp temp [ l ] ;318 }319 }320321 f o r ( j =0; j <3; j++)322 {323 J o [ j ] = (∗ z0+4∗ i ) [ j ] ;324 }325326 f o r ( j =0; j <3; j++)327 {328 ∗ (∗ (∗ ( jacob ian+i i )+i )+j ) = J p [ j ] ;329 ∗ (∗ (∗ ( jacob ian+i i )+i )+j +3) = J o [ j ] ;330 }331 }332 e l s e i f (DH RP[ i ]==0) /∗Prismat ic j o i n t ∗/333 {334335 f o r ( j =0; j <3; j++)336 {337 J p [ j ] = (∗ z0+4∗ i ) [ j ] ;338 J o [ j ] = 0 ;339 }340341 f o r ( j =0; j <3; j++)342 {343 ∗ (∗ (∗ ( jacob ian+i i )+i )+j ) = J p [ j ] ;344 ∗ (∗ (∗ ( jacob ian+i i )+i )+j +3) = J o [ j ] ;

58

345 }346 }347 }348349 i f ( i i <(dof ) )350 {351 f o r ( j =0; j <6; j++)352 {353 f o r ( i=i i +1; i<dof ; i++)354 {355 ∗ (∗ (∗ ( jacob ian+i i )+i )+j ) = 0 ;356 }357 }358 }359 }360361 /∗Dea l l oca te memory∗/362 f r e e ( zt ) ;363 f r e e ( pt ) ;364 f r e e ( p0 ) ;365 f r e e ( z0 ) ;366367 /∗Al l o ca t ing memory f o r the MassMatrix Calcuat ions ∗/368 double ∗ xs i ze , ∗ ys i ze , ∗ z s i z e ;369 double ∗∗∗D temp , ∗∗∗ I I ; //∗∗∗D,370 in t r , m, n ;371 double ∗∗R, ∗∗A n ;372373 double ∗∗∗TransposedJacobian upper ;374 double ∗∗∗TransposedJacobian upper t imesmass ;375 double ∗∗∗TransposedJacobian lower ;376 double ∗∗∗ Jacobian upper ;377 double ∗∗∗ Jacob ian lower ;378379 TransposedJacobian upper = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;380 TransposedJacobian upper t imesmass = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;381 TransposedJacobian lower = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;382 A n = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;383 R = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;384 f o r ( i =0; i<dof ; i++)385 {386 ∗( TransposedJacobian upper t imesmass+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;387 ∗( TransposedJacobian lower+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;388 ∗(A n+i ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;389 ∗(R+i ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;390 ∗( TransposedJacobian upper+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;391 f o r ( r=0; r<dof ; r++)392 {393 ∗(∗( TransposedJacobian upper t imesmass+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;394 ∗(∗( TransposedJacobian lower+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;395 ∗(∗( TransposedJacobian upper+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;396 }397 }398399 Jacobian upper = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;400 Jacob ian lower = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;401 I I = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;402 D temp = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;403 //D = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;404 f o r ( i =0; i<dof ; i++)405 {406 ∗( Jacobian upper+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;407 ∗( Jacob ian lower+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;408 ∗( I I+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;409 ∗(D temp+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;410 //∗(D+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;411 f o r ( r=0; r <3; r++)412 {413 ∗(∗( Jacobian upper+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;414 ∗(∗( Jacob ian lower+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;415 ∗(∗( I I+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;416 }417 f o r ( r=0; r<dof ; r++)418 {419 ∗(∗(D temp+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;420 // ∗(∗(D+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;421 }422 }423424 x s i z e = ( double ∗) mal loc ( dof ∗ s i z e o f ( double ) ) ;425 y s i z e = ( double ∗) mal loc ( dof ∗ s i z e o f ( double ) ) ;426 z s i z e = ( double ∗) mal loc ( dof ∗ s i z e o f ( double ) ) ;427428 /∗Set the l i n k s i z e s ∗/429 f o r ( i =0; i<dof ; i++)430 {431 ∗( x s i z e +0) = 1;// x = [ 3 ; 1 7 ; 1 7 ] ;432 ∗( y s i z e +0) = 1;// y=[1; 1 ; 1 ] ;

59

433 ∗( z s i z e +0) = 1;// z =[1; 1 ; 1 ] ;434 ∗( x s i z e +1) = 3;// x = [ 3 ; 1 7 ; 1 7 ] ;435 ∗( y s i z e +1) = 1;// y=[1; 1 ; 1 ] ;436 ∗( z s i z e +1) = 1;// z =[1; 1 ; 1 ] ;437 ∗( x s i z e +2) = 3;// x = [ 3 ; 1 7 ; 1 7 ] ;438 ∗( y s i z e +2) = 1;// y=[1; 1 ; 1 ] ;439 ∗( z s i z e +2) = 1;// z =[1; 1 ; 1 ] ;440 ∗( x s i z e +3) = 3;// x = [ 3 ; 1 7 ; 1 7 ] ;441 ∗( y s i z e +3) = 1;// y=[1; 1 ; 1 ] ;442 ∗( z s i z e +3) = 1;// z =[1; 1 ; 1 ] ;443 ∗( x s i z e +4) = 3;// x = [ 3 ; 1 7 ; 1 7 ] ;444 ∗( y s i z e +4) = 1;// y=[1; 1 ; 1 ] ;445 ∗( z s i z e +4) = 1;// z =[1; 1 ; 1 ] ;446 ∗( x s i z e +5) = 3;// x = [ 3 ; 1 7 ; 1 7 ] ;447 ∗( y s i z e +5) = 1;// y=[1; 1 ; 1 ] ;448 ∗( z s i z e +5) = 1;// z =[1; 1 ; 1 ] ;449450 /∗Set R to eye−matrix∗/451 f o r (k=0;k<3;k++)452 {453 f o r ( l =0; l <3; l++)454 {455 /∗ I n i t i a l i z e the i n e r t i a matrix to zero ∗/456 ∗ (∗ (∗ ( I I+i )+k)+l ) = 0 ;457 /∗ I n i t i a l i z e the RotationMatrix to zero ∗/458 ∗(∗(R+k)+l ) = 0 ;459 }460 }461 ∗(∗(R+0)+0) = 1 ;462 ∗(∗(R+1)+1) = 1 ;463 ∗(∗(R+2)+2) = 1 ;464 }465466 /∗ Star t c a l c u l a t i o n s ∗/467 f o r ( i =0; i<dof ; i++)468 {469 /∗ s e t the d iagona l terms at the standard va lues ∗/470 ∗ (∗ (∗ ( I I+i )+0)+0) = (∗ (DH mass+i ) /12) ∗ ( ( (∗ ( y s i z e+i ) ) ∗ (∗ ( y s i z e+i ) ) ) + ( (∗ ( z s i z e+i ) ) ∗ (∗ (

z s i z e+i ) ) ) ) ;471 ∗ (∗ (∗ ( I I+i )+1)+1) = (∗ (DH mass+i ) /12) ∗ ( ( (∗ ( x s i z e+i ) ) ∗ (∗ ( x s i z e+i ) ) ) + ( (∗ ( z s i z e+i ) ) ∗ (∗ (

z s i z e+i ) ) ) ) ;472 ∗ (∗ (∗ ( I I+i )+2)+2) = (∗ (DH mass+i ) /12) ∗ ( ( (∗ ( x s i z e+i ) ) ∗ (∗ ( x s i z e+i ) ) ) + ( (∗ ( y s i z e+i ) ) ∗ (∗ (

y s i z e+i ) ) ) ) ;473474 f o r (m=0;m<3;m++)475 {476 f o r (n=0;n<3;n++)477 {478 ∗(∗(A n+m)+n) = ∗ (∗ (∗ (A+i )+m)+n) ; //∗ (∗ (∗ (A+(dof−( i +1) ) )+m)+n) ;//∗ (A+(dof−1) ) [ n ] [m

] ; / / [ n ] ;479 }480 }481 R = MatrixMatrix (R, A n , 3 , 3 , 3 ) ;//2eR482483 /∗Because Jacobian i s de f ined Transposed i t should be transposed to get the normal∗/484 f o r ( l =0; l <3; l++)485 {486 f o r (k=0;k<dof ; k++)487 {488 ∗ (∗ (∗ ( Jacob ian lower+i )+l )+k) = ∗ (∗ (∗ ( jacob ian+i )+k)+l +3) ;489 ∗ (∗ (∗ ( Jacobian upper+i )+l )+k) = ∗ (∗ (∗ ( jacob ian+i )+k)+l ) ;490 }491 }492493 ∗( TransposedJacobian upper+i ) = TransposeMatrix (∗ ( Jacobian upper+i ) ,3 , dof ) ;494 ∗( TransposedJacobian lower+i ) = TransposeMatrix (∗ ( Jacob ian lower+i ) ,3 , dof ) ;495 f o r ( l =0; l <3; l++)496 {497 f o r (k=0;k<dof ; k++)498 {499 (∗ (∗ (∗ ( TransposedJacobian upper t imesmass+i )+k)+l ) ) = ∗(DH mass+i )∗ (∗ (∗ (∗ (

TransposedJacobian upper+i )+k)+l ) ) ;500 }501 }502 ∗(D temp+i ) = MatrixMatrix (∗ ( TransposedJacobian upper t imesmass+i ) ,∗ ( Jacobian upper+i ) , dof , 3 ,

dof ) ;503504 ∗(D+i ) = MatrixMatrix ( MatrixMatrix ( MatrixMatrix ( MatrixMatrix (∗ ( TransposedJacobian lower+

i ) ,R, dof , 3 , 3 ) ,∗ ( I I+i ) , dof , 3 , 3 ) , TransposeMatrix (R, 3 , 3 ) , dof , 3 , 3 ) ,∗ ( Jacob ian lower+i ) , dof , 3 , dof) ;

505506507 f o r (n=0;n<dof ; n++)508 {509 f o r (m=0;m<dof ;m++)510 {511 ∗ (∗ (∗ (D+i )+m)+n) = ∗ (∗ (∗ (D+i )+m)+n) + ∗ (∗ (∗ (D temp+i )+m)+n) ;512 }

60

513 }514515 i f ( i >0)516 {517 f o r (n=0;n<dof ; n++)518 {519 f o r (m=0;m<dof ;m++)520 {521 ∗ (∗ (∗ (D+i )+m)+n) = ∗ (∗ (∗ (D+i −1)+m)+n) + ∗ (∗ (∗ (D+i )+m)+n) ;522 }523 }524 }525 }526 ///Attempting to f r e e an i n v a l i d po in t e r ( a po in t e r to a memory block that was not a l l o c a t e d by ca l l o c ,

malloc , or r e a l l o c ) may a f f e c t subsequent a l l o c a t i o n r eque s t s and cause e r r o r s .527528 /∗PRBLEMS WITH THE DEALLOCATION OF MEMORY THE PARTS COMMENTED OUT WORK WHEN YOU COPY THE ENTIRE

DEALLOCATION TO JUST BELOW THE ALLOCATION OF THE MEMORY∗/529 /∗Dea l l o ca t i on o f memory∗/530 f o r ( i =0; i <3; i++)531 {532 f o r ( r=0; r<dof ; r++)533 {534 f r e e (∗ (∗ ( TransposedJacobian upper+i )+r ) ) ;535 // f r e e (∗ (∗ ( TransposedJacobian upper t imesmass+i )+r ) ) ;536 f r e e (∗ (∗ ( TransposedJacobian lower+i )+r ) ) ;537 }538 // f r e e (∗ (A n+i ) ) ;539 // f r e e (∗ (R+i ) ) ;540 }541 f o r ( i =0; i<dof ; i++)542 {543 f o r ( r=0; r <3; r++)544 {545 // f r e e (∗ (∗ ( Jacobian upper+i )+r ) ) ;546 // f r e e (∗ (∗ ( Jacob ian lower+i )+r ) ) ;547 // f r e e (∗ (∗ ( I I+i )+r ) ) ;548 }549 f o r ( r=0; r<dof ; r++)550 {551 f r e e (∗ (∗ (D temp+i )+r ) ) ;552 // f r e e (∗ (∗ (D+i )+r ) ) ; // f r e e−ed in the main f i l e553 }554 }555 f r e e ( x s i z e ) ;556 f r e e ( y s i z e ) ;557 f r e e ( z s i z e ) ;558 /∗THE LOWER PART IS NOT WORKING AT ALL∗/559 ////560 //// f o r ( i =0; i <3; i++)561 //// {562 //// // f r e e (∗ ( TransposedJacobian upper+i ) ) ;563 //// }564 ////// f r e e ( TransposedJacobian upper ) ;565 ////// f r e e ( TransposedJacobian upper t imesmass ) ;566 ////// f r e e ( TransposedJacobian lower ) ;567 //// // f r e e ( Jacobian upper ) ;568 //// // f r e e ( Jacob ian lower ) ;569 //// // f r e e ( I I ) ;570 //// // f r e e (D temp) ;571 //// // f r e e (D) ;572 } /∗End MassMatrix∗/573574 double DET33( double ∗∗B)575 {576 double outputdet ;577 double ∗∗det ;578 in t s i z e =3;579 in t i , j , k ;580 det = ( double ∗∗) mal loc ( s i z e ∗ s i z e o f ( double ∗) ) ;581 f o r ( i =0; i<s i z e ; i++)582 {583 ∗( det+i ) = ( double ∗) mal loc ( s i z e ∗ s i z e o f ( double ) ) ;584 }585586 f o r ( i =0; i<s i z e ; i++)587 {588 f o r ( j =0; j<s i z e ; j++)589 {590 ∗(∗( det+i )+j ) = 0 ;591 }592 }593 /∗ the f i r s t row i s used to c a l c u l a t e the c o f a c t o r s ∗/594 /∗Cal cu la t e s the det f o r 3∗3 matr i ces ∗/595596 f o r ( j =0; j <( s i z e −1) ; j++)597 {598 f o r ( i =0; i<s i z e ; i++)

61

599 {600 i f ( j==0) /∗Calcu la te the f i r s t ba s i c determinats o f the 2∗2 matr i ces ∗/601 {602 i f ( i==0)603 {604 ∗(∗( det+i )+j ) = ( (∗ (∗ (B+j+s i z e −2)+i +1) ) ∗ (∗ (∗ (B+j+1+s i z e −2)+i +2) ) ) −((∗(∗(B+j+

s i z e −2)+i +2) ) ∗ (∗ (∗ (B+j+1+s i z e −2)+i +1) ) ) ;605 }606 e l s e607 {608 i f ( i <( s i z e −1) )609 {610 ∗(∗( det+i )+j ) = ( (∗ (∗ (B+j+s i z e −2)+i −1) ) ∗ (∗ (∗ (B+j+1+s i z e −2)+i +1) ) ) −((∗(∗(B+j

+s i z e −2)+i +1) ) ∗ (∗ (∗ (B+j+1+s i z e −2)+i −1) ) ) ;611 }612 e l s e /∗ i =( s i z e −1)=lastco lumn ∗/613 {614 ∗(∗( det+i )+j ) = ( (∗ (∗ (B+j+s i z e −2)+i−s i z e +1) ) ∗ (∗ (∗ (B+j+1+s i z e −2)+i−s i z e +2)

) ) − ( (∗ (∗ (B+j+s i z e −2)+i−s i z e +2) ) ∗ (∗ (∗ (B+j+1+s i z e −2)−s i z e+i +1) ) ) ;615 }616 }617 }618 e l s e /∗ j >0 ba s i c l y the determinants o f 3∗3∗/619 {620 i f ( i==0)621 {622 ∗(∗( det+i )+j ) = ( (∗ (∗ (B−j+s i z e −2)+i ) )∗ (∗ (∗ ( det+i )+j−1) ) ) ;623 }624 e l s e625 {626 i f ( i <( s i z e −1) )627 {628 ∗(∗( det+i )+j ) = ( (∗ (∗ (B−j+s i z e −2)+i ) )∗ (∗ (∗ ( det+i )+j−1) ) ) ;629 }630 e l s e /∗ i =( s i z e −1)=lastco lumn ∗/631 {632 ∗(∗( det+i )+j ) = ( (∗ (∗ (B−j+s i z e −2)+i ) )∗ (∗ (∗ ( det+s i z e −1)+j−1) ) ) ;633 f o r (k=1;k<s i z e ; k++)634 {635 double f a c t o r =1;636 in t n ;637 f o r (n=0;n<k ; n++)638 {639 f a c t o r = −1∗ f a c t o r ;640 }641 ∗(∗( det+i )+j ) = (∗ (∗ ( det+i )+j ) ) + f a c t o r ∗ (∗ (∗ ( det+i−k )+j ) ) ;642 }643 }644 }645 }646 }647 }648649 outputdet = ∗(∗( det+2)+1) ;650 return outputdet ;651 f o r ( i =0; i<s i z e ; i++)652 {653 f r e e (∗ ( det+i ) ) ;654 }655 f r e e ( det ) ;656 }657658 double Determinant ( double ∗∗B, i n t rows ) /∗Calcu la te the Determinant o f a Matrix∗/659 {660 double outputdet ;661 double ∗det ;662 double ∗∗∗∗M;663 in t s i z e ;664 s i z e=rows ;665 in t i , j , k , l ,m, n , r=0;666 in t f =1;667668 f o r ( i =0; i <( s i z e −3) ; i++)669 {670 f=f ∗( s i z e−i ) ;671 }672 det = ( double ∗) mal loc ( f ∗ s i z e o f ( double ) ) ;673674 M = ( double ∗∗∗∗) mal loc ( s i z e o f ( double ∗∗∗) ) ;675 f o r ( i =0; i<s i z e ; i++)676 {677 ∗(M+i ) = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;678 f o r ( j =0; j<s i z e ; j++) //3??679 {680 ∗(∗(M+i )+j ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;681 f o r (m=0;m<s i z e ;m++) //3??682 {683 ∗ (∗ (∗ (M+i )+j )+m) = ( double ∗) mal loc ( s i z e ∗ s i z e o f ( double ) ) ;

62

684 }685 }686 }687688 f o r ( i =0; i<f ; i++)689 {690 (∗ ( det+i ) ) = 0 ;691 }692693 i f ( rows==1)694 {695 outputdet = ∗(∗(B) ) ;696 }697 i f ( rows==2)698 {699 outputdet = ( (∗ (∗ (B+0)+0) ) ∗ (∗ (∗ (B+1)+1) ) ) −((∗(∗(B+1)+0) ) ∗ (∗ (∗ (B+0)+1) ) ) ;700 }701702 i f ( rows >2) /∗matrix at l e a s t 3∗3 ∗/703 {704 f o r ( i =0; i <( s i z e −3) ; i++) /∗ i=s i z e o f the part (4x4−>nxn ) matrix∗/705 {706 f o r ( l =0; l <( s i z e ) ; l++)707 {708 f o r (n=0;n<( s i z e −3) ; n++) /∗n=s i z e o f the matrix∗/709 {710 r++;711 // s e l e c t the 3∗3 matr i ces712 // ∗(∗(∗(∗M+i )+m) ) )= ∗∗B713 f o r (m=0;m<3;m++) /∗m=s i z e o f the matrix∗/714 {715 f o r ( j =0; j <3; j++)716 {717 i f (n==0) //( s i z e−l )718 {719 i f ( ( s i z e−l )<=3)//(( s i z e−l )<=3)720 {721 //∗ (∗ (∗ (∗ (M+i )+n)+m)+j ) = ∗(∗(B+rows−3+m)+j +1) ;722 i f ( ( s i z e−l )==0) /∗can ’ t happen∗/723 {724 in t e r r o r ;725 e r r o r = 1 ;726 }727 i f ( ( s i z e−l )==1)728 {729 /∗∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+l+j+1+4−s i z e ) ;730 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+l+j+1+3−s i z e ) ;731 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+l+j+1+2−s i z e ) ;∗/732 //∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+l+0−3) ;733 //∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+l+1+0) ;734 //∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+l+1+1) ;735 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0−0) ;736 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+0) ;737 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+1+1) ;738 }739 i f ( ( s i z e−l )==2)740 {741 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0) ;742 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1) ;743 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+l +1) ;744 }745 i f ( ( s i z e−l )==3)746 {747 // i f ( j >=1)748 //{749 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0) ;750 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+l ) ;751 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+2+l ) ;// l+1+1752 //}753 // e l s e754 //{755 // i f (m>=l )756 // {757 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+j ) = ∗(∗(B+rows−3+m+j )−1) ;758 //∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m+1)+0) ;759 //∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m+2)+0) ;760 // }761 // i f (m<l )762 // {763 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+l−1−m) ;764 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+l +1) ;765 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+l +2) ;766 //}767 // e l s e768 //{769 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0−1) ;770 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+0) ;771 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+2+0) ;

63

772 //}773 // //∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+l ) ;774 // //∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+1+l ) ;// l+1+1775 //}776 }777 /∗ o r i ∗/778 //∗ (∗ (∗ (∗ (M+i )+l )+m)+j ) = ∗(∗(B+rows−3+m)+l+j +1) ;779780 }781 e l s e /∗ h i e r gebleven ∗/782 {783 ∗ (∗ (∗ (∗ (M+i )+l )+m)+j ) = ∗(∗(B+rows−3+m)+l+j +1) ;784 // i f ( l==0)785 //{786 //787 // //∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+l−1+1) ;788 // //∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+l+1+1) ;789 // //∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+l+2+1) ;790 //}791 // e l s e792 //{793 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+l−1+1) ;794 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+l+1+1) ;795 // ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+l+2+1) ;796 //}797 }798 }799 e l s e800 {801 i f (n<3) // i =1,2802 {803 i f (n==2)804 {805 i f ( ( s i z e−l )<=3)806 {807 i f ( ( s i z e−l )==3)808 {809 i f (n<=l )810 {811 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+l −2);//+ j−2)

;812 i f (n==l )813 {814 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)−1+l ) ;//+

j−1) ;815 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+1+l ) ;//+

j +1) ;816 }817 e l s e818 {819 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+l ) ;//+

j−1) ;820 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+2+l ) ;//+

j +1) ;821 }822 }823 e l s e /∗n>l ∗/824 {825 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0);//+ j−2) ;826 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+l ) ;//+ j−1)

;827 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+2+l ) ;//+ j +1)

;828 }829 }830 i f ( ( s i z e−l )==2)831 {832 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+n−1);//+ j−2) ;833 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+n) ;//+1//+ j−1) ;834 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+n+2);//+2+ l //+ j

+1) ;835 }836 i f ( ( s i z e−l )==1)837 {838 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0);//+ j−2) ;839 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+n) ;//+1//+ j−1) ;840 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+n+1);//+2+ l //+ j

+1) ;841 }842 }843 e l s e844 {845 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0+l ) ;//+ j−2) ;846 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+l ) ;//+ j−1) ;847 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+3+l ) ;//+ j +1) ;848 }849 }850 e l s e /∗n==1∗/

64

851 {852 i f ( ( s i z e−l )<=3)853 {854 i f ( ( s i z e−l )==3)855 {856 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0);//+ j ) ;857 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1);//+ j +1) ;858 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+2+l ) ;//+ j +2) ;859 }860 i f ( ( s i z e−l )==2)861 {862 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+l−n−2);//+ j ) ;863 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+l−n−1);//+ j +1) ;864 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+l−n) ;//+ j +2) ;865 }866 i f ( ( s i z e−l )==1)867 {868 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0+n) ;//+ j ) ;869 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+n) ;//+ j +1) ;870 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+2+n) ;//+ j +2) ;871 }872 i f ( ( s i z e−l )==0)873 {874 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0);//+ j ) ;875 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+l ) ;//+ j +1) ;876 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+3+l ) ;//+ j +2) ;877 }878 }879 e l s e880 {881 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0+l ) ;//+ j ) ;882 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+2+l ) ;//+ j +1) ;883 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+3+l ) ;//+ j +2) ;884 }885 }886 }887 e l s e /∗ l a s t column∗/888 {889 i f ( ( s i z e−l )<=3)890 {891 i f ( ( s i z e−l )==3)892 {893 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)−1+l ) ;//+ j ) ;894 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+l ) ;//+ j +1) ;895 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+2+l ) ;//+ j +2) ;896 }897 i f ( ( s i z e−l )==2)898 {899 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)−n+l ) ;//−2+ l ) ;//+ j ) ;900 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)−1+l ) ;//−1+ l ) ;//+ j +1)

;901 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+1+l ) ;//+1+ l ) ;//+ j +2)

;902 }903 i f ( ( s i z e−l )==1)904 {905 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0);//+ j ) ;906 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1);//+ j +1) ;907 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+n) ;//+ j +2) ;908 }909 i f ( ( s i z e−l )==0)910 {911 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0);//+ j ) ;912 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+l ) ;//+ j +1) ;913 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+3+l ) ;//+ j +2) ;914 }915 }916 e l s e917 {918 ∗ (∗ (∗ (∗ (M+i )+l )+m)+0) = ∗(∗(B+rows−3+m)+0+l ) ;//+ j−2) ;919 ∗ (∗ (∗ (∗ (M+i )+l )+m)+1) = ∗(∗(B+rows−3+m)+1+l ) ;//+ j−1) ;920 ∗ (∗ (∗ (∗ (M+i )+l )+m)+2) = ∗(∗(B+rows−3+m)+2+l ) ;//+ j +1) ;921 }922 //∗ (∗ (∗ (∗ (M+i )+n)+m)+j ) = ∗(∗(B+rows−3+m)+j−3) ;923 }924 }925 }926 }927 ∗( det+(r−1) ) = DET33(∗ (∗ (M+i )+l ) ) ;928929930 //∗( det+r ) = DET33(B) ;931932 outputdet = 0 ;933 // }934 }935 }936 }

65

937 }938939 // f o r ( i=s t a r t i nd ex +1; i < m NumRows ; i++)940 // {941 // f o r ( j=s t a r t i nd ex +1; j < m NumRows ; j++)942 // {943 // m tmpPolyf = m Mat [ i ] [ s t a r t i nd ex ] ∗ m Mat [ s t a r t i nd ex ] [ j ] ;944 // m Mat [ i ] [ j ] ∗= m Mat [ s t a r t i nd ex ] [ s t a r t i nd ex ] ;945 // m Mat [ i ] [ j ] −= m tmpPolyf ;946 // } // end f o r j947 // } // end f o r i948 // r e t v a l = CalcDetRecurse ( s t a r t i nd ex + 1) ;949 outputdet = ∗( det+0) ;950 /∗ dea l l o c a t i n g memory∗/951952 f o r ( i =0; i<s i z e ; i++)953 {954 f o r ( j =0; j<s i z e ; j++)955 {956 f o r (m=0;m<s i z e ;m++)957 {958 f r e e (∗ (∗ (∗ (M+i )+j )+m) ) ;959 }960 // f r e e (∗ (∗ (M+i )+j ) ) ;961 }962 // f r e e (∗ (M+i ) ) ;963 }964 // f r e e (M) ;965 f r e e ( det ) ;966 return outputdet ;967 } /∗End Determinant∗/968969 void inver seMatr ix ( double ∗∗D, in t s i z e ) /∗Calcu la te the i nv e r s e o f a Matrix∗/970 {971 double Det ;972 //Det = Determinant (D, s i z e ) ;973 } /∗End o f i nv e r t i n g the Matrix∗/974975976 /∗ Star t main loop ∗/977 in t tmain ( i n t argc , TCHAR∗ argv [ ] )978 {979980 in t dof = 6 ; /∗Number o f l i n k s ∗/981 double ∗DH a , ∗DH d , ∗DH alpha , ∗DH theta , ∗DH WD, ∗DH mass ;982 in t ∗DH RP;983 /∗ Jacobian matrix 6xn∗/984 double ∗∗∗ jacobian , ∗∗∗D;985 in t i , rows = 6 , c o l s =2;986 in t r ;987 /∗ memory a l l o c a t i o n ∗/988 /∗ ∗∗∗ Jacobian = 3D Matrix = s t ru c tu r e in Matlab∗/989 jacob ian = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;990 f o r ( i =0; i <6; i++)991 {992 ∗( jacob ian+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;993 f o r ( r=0; r<dof ; r++)994 {995 ∗(∗( jacob ian+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;996 }997 }998999 D = ( double ∗∗∗) mal loc ( s i z e o f ( double ∗∗) ) ;

1000 f o r ( i =0; i<dof ; i++)1001 {1002 ∗(D+i ) = ( double ∗∗) mal loc ( s i z e o f ( double ∗) ) ;1003 f o r ( r=0; r<dof ; r++)1004 {1005 ∗(∗(D+i )+r ) = ( double ∗) mal loc ( s i z e o f ( double ) ) ;1006 }1007 }10081009 DH a = ( double ∗) mal loc ( dof ∗ s i z e o f ( double ) ) ;1010 DH d = ( double ∗) mal loc ( dof ∗ s i z e o f ( double ) ) ;1011 DH alpha = ( double ∗) mal loc ( dof ∗ s i z e o f ( double ) ) ;1012 DH theta = ( double ∗) mal loc ( dof ∗ s i z e o f ( double ) ) ;1013 DH RP = ( in t ∗) mal loc ( dof ∗ s i z e o f ( i n t ) ) ;1014 DHWD = ( double ∗) mal loc ( dof ∗ s i z e o f ( double ) ) ;1015 DH mass = ( double ∗) mal loc ( dof ∗ s i z e o f ( double ) ) ;1016 A = ( trans format ionMatr ix ∗) mal loc ( dof ∗ s i z e o f ( t rans format ionMatr ix ) ) ;10171018 double t1 =0.14∗pi , t2 =0.12∗ pi ;1019 /∗ i n i t i a l i z a t i o n s ∗/1020 ∗(DH a+0)= 1 . 0 ; ∗(DH a+1)= 3 . 0 ; ∗(DH a+2)= 3 . 0 ; ∗(DH a+3)= 3 . 0 ; ∗(DH a+4)=

3 . 0 ; ∗(DH a+5)= 3 . 0 ;1021 ∗(DH d+0)= 0 . 0 ; ∗(DH d+1)= 0 . 0 ; ∗(DH d+2)= 0 . 0 ; ∗(DH d+3)= 0 . 0 ; ∗(DH d+4)=

0 . 0 ; ∗(DH d+5)= 0 . 0 ;

66

1022 ∗(DH alpha+0)= 0 . 0 ; ∗(DH alpha+1)= 0 . 0 ; ∗(DH alpha+2)= 0 . 0 ; ∗(DH alpha+3)= 0 . 0 ; ∗(DH alpha+4)= 0 . 0 ; ∗(DH alpha+5)= 0 . 0 ;

1023 ∗(DH theta+0)= t1 ; ∗(DH theta+1)= t2 ; ∗(DH theta+2)= t2 ; ∗(DH theta+3)= t2 ; ∗(DH theta+4)= t2 ; ∗(DH theta+5)= t2 ;

1024 ∗(DH RP+0)= 1 ; ∗(DH RP+1)= 1 ; ∗(DH RP+2)= 1 ; ∗(DH RP+3)= 1 ; ∗(DH RP+4)= 1 ; ∗(DH RP+5)= 1 ;

1025 ∗(DHWD+0)= 0 . 5 ; ∗(DHWD+1)= 0 . 5 ; ∗(DHWD+2)= 0 . 5 ; ∗(DHWD+3)= 0 . 5 ; ∗(DHWD+4)= 0 . 5 ; ∗(DHWD+5)= 0 . 5 ; / / weight d i s t r i b u t i o n

1026 ∗(DH mass+0)= 1 ; ∗(DH mass+1)= 3 ; ∗(DH mass+2)= 3 ; ∗(DH mass+3)= 3 ; ∗(DH mass+4)= 3 ; ∗(DH mass+5)= 3 ;

10271028 computeMassMatrix (D, jacobian , rows , co l s , DH a , DH d , DH alpha , DH theta , DH RP, DH WD, DH mass ,

dof ) ;1029 // inver seMatr ix (∗ (D+(dof−1) ) , dof ) ;1030 /∗ memory d e a l l o c a t i o n ∗/1031 f r e e (DH a) ;1032 f r e e (DH d) ;1033 f r e e (DH alpha ) ;1034 f r e e ( DH theta ) ;1035 f r e e (DH RP) ;1036 f r e e (DHWD) ;1037 f r e e (DH mass ) ;1038 f r e e (A) ;10391040 f o r ( i =0; i <6; i++)1041 {1042 f o r ( r=0; r<dof ; r++)1043 {1044 //( CrtIsVal idHeapPointer ( (∗ (∗ ( jacob ian+i )+r ) ) ) ) ;1045 // f r e e (∗ (∗ ( jacob ian+i )+r ) ) ;1046 }1047 // f r e e (∗ ( jacob ian+i ) ) ;1048 }1049 // f r e e ( jacob ian ) ;1050 f o r ( i =0; i<dof ; i++)1051 {1052 f o r ( r=0; r<dof ; r++)1053 {1054 f r e e (∗ (∗ (D+i )+r ) ) ;1055 }1056 }1057 return 0 ;1058 } /∗End Main loop ∗/

67

Visualizing and simulating a serial linkage defined using ...mate.tue.nl/mate/pdfs/10633.pdf ·...

Documents

Transcript of Visualizing and simulating a serial linkage defined using ...mate.tue.nl/mate/pdfs/10633.pdf ·...