Load a STEP file and simulate a robot (demo_CAS_robot.cpp)

Tutorial that teaches how to use the CASCADE module to load a 6-DOF robot saved in a STEP file as an assembly, exported from a CAD model.

This is an advanced tutorial that shows how to load complex 3D models that have been saved in STEP file format from professional 3D CAD software.

Most high-end 3D CAD software can save assemblies and parts in STEP format. Among the most used CAD software, we cite CATIA, SolidEdge, SolidWorks, PTC Pro/E, etc. The following tutorial is based on SolidEdge, but it can be adapted to other CADs with minor modifications.

Prepare a STEP file

The CASCADE module is able to directly parse sub-assemblies and sub-parts that have been saved into a single STEP file. However, it is better to prepare the assembly with certain guidelines, before saving it: noticeably, we will put some 'auxiliary' objects in the assembly (the markers) that we will use from the C++ programming side in order to retrieve useful coordinates for building the constraints between the parts. This process is explained in the folowing example.

Creating a demo mechanism (a car, a robot, etc.) might take hours. To make things simplier, we load a ready-to-use model that can be downloaded from the website of a manufaturer of industrial robots, see download page from the ABB website. We downloaded the 3D model for the IRB 7600 robot: by the way it is already in STEP format, so it could be loaded directly by the CASCADE unit, but we prefer to mofify it in SolidEdge and save it again.
  • Start SolidEdge (we use the v18 version in this example)
  • Menu File / Open...
  • Choose the STEP (.stp or .step) file of the robot, as downloaded from the ABB site.
  • In the 'New' window, in the 'General' tab, select 'Normal.asm' to let SolidEdge create an assembly from the STEP file
  • Press OK. After the conversion, SolidEdge created the assembly. Look into the directory of your STEP file: SolidEdge created its .asm and many .part files. The Assembly PathFinder panel, to the right, shows all the parts in the assembly:

  • Note that the assembly is made of many parts. Maybe that there is not a one-to-one correspondence from rigid bodies for our simulation, and CAD parts, so we want to organize N sub-assemblies, that represent our N rigid bodies in the simulation. That is, it would be nice if each part stays in a separate sub-assebly that represents a 'rigid body', and such subassemblies can optionally contain some auxiliary objects (we call them 'markers') that can be used later on the C++ side to find the position of the joints.
  • To make the sub assembles:
    • Select the 'Parts library' panel,
    • press button 'Create in Place' to open the create window,
    • set 'Normal.asm' as Template
    • set 'Base' (or 'Forearm', or other meaningful name) in 'New file name',
    • browse to your directory where you have all other assembly files, in 'New file location',
    • menu File / Close and return, to go back to general assembly.
    • Repeat the last steps to create the other subassemblies, named for example 'Bicep', 'Forearm', 'Wrist', etc.
  • The created subassemblies are still empty. So we must move the imported parts into them. Drag and drop a part from parent assembly to a child asembly does not work in SolidEdge v.18, yet a simple way to do this is to
    • select the part in the parent assembly,
    • press Ctrl+C to copy,
    • then select the sublevel in the Assembly PathFinder,
    • use 'Edit' from popup-menu,
    • press Ctrl-V to paste it,
    • then go back to parent assembly with menu 'File / Close and return'.
  • Repeat this until you have all the parts in their separate subassemblies. Some subassembly might have multiple parts. Look our example:

  • Now we add an auxiliary part in all subassemblies, to represent the coordinates where you want the constraints (links, motors, ..) to be created using C++ functions. These are like 'placeholders'. To do so, we create a 'marker.par' part, such as the one in the following figure:

    marker.par

  • We insert this marker part in all subassemblies, properly aligned to all the joints:

  • At the end, the Assembly PathFinder panel should show something like the following:

  • Select the base assembly, use the menu File / Save as.. and choose STEP as file format, then save the entire assembly.
  • Quit the SolidEdge software.

Write C++ code to load the model

The key of the remaining process is the functionality of the ChCascadeDoc class. Such class has the functionality of loading sub-assemblies from a STEP file by using the function mydoc.GetNamedShape(...) that takes, as argument, the ASCII name of the subassembly (or sub part).

A small inconvenience happens here: because of a SolidEdge-specific issue, the names of the subassemblies in the STEP file are not always the same names that you read in the Assembly PAthFinder window. In detail, all the names of the assemblies are automatically translated to Assem1, Assem2, etc., whereas you would expect the names of the assemblies that you created, such as Base, Turret, etc.

A workaround to this inconvenience is the following: you use the mydoc.Dump(GetLog()) function to print the hierarchy on the console and take note of the STEP names on a piece of paper (or just use the demo_converter.exe to show that hierarchy), you will see something like:

-Name :Assem10 (root)
pos at: 0 0 0 (absolute)
pos at: 0 0 0 (.Location)
-Name :Assem8
pos at: 0 0 0 (absolute)
pos at: 0 0 0 (.Location)
-Name :IRB7600_23_500_m2000_rev1_01-1
pos at: 0 0 0 (absolute)
pos at: 0 0 0 (.Location)
-Name :marker
pos at: 2.29901e-035 -2.99071e-036 0.2185 (absol
pos at: 2.29901e-035 -2.99071e-036 0.2185 (.Loca
-Name :Assem4
pos at: 0 0 0 (absolute)
pos at: 0 0 0 (.Location)
-Name :IRB7600_23_500_m2000_rev1_01-2
pos at: 0 0 0 (absolute)
pos at: 0 0 0 (.Location)
-Name :marker
pos at: 3.88578e-015 -2.66454e-015 0.2135 (absol
pos at: 3.88578e-015 -2.66454e-015 0.2135 (.Loca
-Name :marker
pos at: 0.41 -0.049 0.78 (absolute)
pos at: 0.41 -0.049 0.78 (.Location)
-Name :marker
pos at: -0.38 -0.03 0.6545 (absolute)
pos at: -0.38 -0.03 0.6545 (.Location)
-Name :Assem1
.... etc. etc. .... ........

From the example above, you see that Base has become Assem8, Turret has become Assem4, and so on. (luckily enough, SolidEdge did not change the name of the parts, only the assembly names were changed). Take note of this on a sheet of paper.

Now, let's develop the C++ program that loads the robot model and simulates it.

First of all, include the needed libraries (note the unit_CASCADE/... headers) and use the proper namespaces:

#include "physics/CHapidll.h"
#include "core/CHrealtimeStep.h"
#include "irrlicht_interface/CHirrAppInterface.h"
#include "irrlicht_interface/CHbodySceneNodeTools.h"
#include "unit_CASCADE/CHcascadeDoc.h"
#include "unit_CASCADE/CHCascadeMeshTools.h"
#include "unit_CASCADE/CHirrCascadeMeshTools.h"
#include "unit_CASCADE/CHirrCascade.h"
#include "irrlicht_interface/CHbodySceneNode.h"
#include <irrlicht.h>
// Use the namespace of Chrono
using namespace chrono;
// Use the main namespaces of Irlicht
using namespace irr;
using namespace core;
using namespace scene;
using namespace video;
using namespace io;
using namespace gui;
// Use the namespace with OpenCascade stuff
using namespace cascade;

Here is the program (a simple demo where the robot is created from the STEP file, constraints are added, and the robot simulation is displayed while moving on a simple trajectory).

int main(int argc, char* argv[])
{
ChGlobals* GLOBAL_Vars = DLL_CreateGlobals();
// 1- Create a ChronoENGINE physical system: all bodies and constraints
// will be handled by this ChSystem object.
ChSystem my_system;
// Create the Irrlicht visualization (open the Irrlicht device,
// bind a simple user interface, etc. etc.)
ChIrrAppInterface application(&my_system, L"Load a robot model from STEP file",core::dimension2d<u32>(800,600),false, true, video::EDT_OPENGL);
// Easy shortcuts to add logo, camera, lights and sky in Irrlicht scene:
//ChIrrWizard::add_typical_Logo(application.GetDevice());
ChIrrWizard::add_typical_Sky(application.GetDevice());
ChIrrWizard::add_typical_Lights(application.GetDevice(), core::vector3df(30,100,30), core::vector3df(30,-80,-30),200,130);
ChIrrWizard::add_typical_Camera(application.GetDevice(), core::vector3df(0.2,1.6,-3.5));

Create the ChCascadeDoc, a container that loads the STEP model and manages its subassembles. Also, prepare some pointers for bodies that will be created.

ChCascadeDoc mydoc;

Also, prepare some pointers for bodies that will be created.

ChBodySceneNodeAuxRef* mrigidBody_base = 0;
ChBodySceneNodeAuxRef* mrigidBody_turret = 0;
ChBodySceneNodeAuxRef* mrigidBody_bicep = 0;
ChBodySceneNodeAuxRef* mrigidBody_elbow = 0;
ChBodySceneNodeAuxRef* mrigidBody_forearm = 0;
ChBodySceneNodeAuxRef* mrigidBody_wrist = 0;
ChBodySceneNodeAuxRef* mrigidBody_hand = 0;
ChBodySceneNodeAuxRef* mrigidBody_cylinder = 0;
ChBodySceneNodeAuxRef* mrigidBody_rod = 0;

Load the STEP model using this command: (be sure to have the STEP file on the hard disk)

bool load_ok = mydoc.Load_STEP("..\\data\\cascade\\IRB7600_23_500_m2000_rev1_01_decorated.stp");

Print the contained shapes, showing the assembly hierarchy:

mydoc.Dump(GetLog());
ChCollisionModel::SetDefaultSuggestedEnvelope(0.002);
ChCollisionModel::SetDefaultSuggestedMargin(0.001);
// In most CADs the Y axis is horizontal, but we want it vertical.
// So define a root transformation for rotating all the imported objects.
ChQuaternion<> rotation1;
rotation1.Q_from_AngAxis(-CH_C_PI/2, VECT_X); // 1: rotate 90° on X axis
ChQuaternion<> rotation2;
rotation2.Q_from_AngAxis(CH_C_PI, VECT_Y); // 2: rotate 180° on vertical Y axis
ChQuaternion<> tot_rotation = rotation2 % rotation1; // rotate on 1 then on 2, using quaternion product
ChFrameMoving<> root_frame( ChVector<>(0,0,0), tot_rotation);

Retrieve some sub shapes from the loaded model, using the GetNamedShape() function, that can use path/subpath/subsubpath/part syntax and * or ? wldcards, etc. Using the / slash is like addressing a Unix directory (in fact the STEP file is organized like a directory with subdirectory, each representing a subassembly).

if (load_ok)
{
TopoDS_Shape shape_base;
if (mydoc.GetNamedShape(shape_base, "Assem10/Assem8" ))
{
// Add the shape to the Irrlicht system, to get also visualization.
mrigidBody_base = (ChBodySceneNodeAuxRef*)addChBodySceneNode_Cascade_C(
&my_system, application.GetSceneManager(),
shape_base);
// The base is fixed to the ground
mrigidBody_base->GetBody()->SetBodyFixed(true);
// Move the body as for global displacement/rotation by pre-transform its coords.
// Note, it could be written also as mrigidBody_base->GetBody() %= root_frame;
mrigidBody_base->GetBody()->ConcatenatePreTransformation(root_frame);
}
else GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_turret;
if (mydoc.GetNamedShape(shape_turret, "Assem10/Assem4" ))
{
// Add the shape to the Irrlicht system, to get also visualization.
mrigidBody_turret = (ChBodySceneNodeAuxRef*)addChBodySceneNode_Cascade_C(
&my_system, application.GetSceneManager(),
shape_turret);
// Move the body as for global displacement/rotation
mrigidBody_turret->GetBody()->ConcatenatePreTransformation(root_frame);
}
else GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_bicep;
if (mydoc.GetNamedShape(shape_bicep, "Assem10/Assem1" ))
{
// Add the shape to the Irrlicht system, to get also visualization.
mrigidBody_bicep = (ChBodySceneNodeAuxRef*)addChBodySceneNode_Cascade_C(
&my_system, application.GetSceneManager(),
shape_bicep);
// Move the body as for global displacement/rotation
mrigidBody_bicep->GetBody()->ConcatenatePreTransformation(root_frame);
}
else GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_elbow;
if (mydoc.GetNamedShape(shape_elbow, "Assem10/Assem5" ))
{
// Add the shape to the Irrlicht system, to get also visualization.
mrigidBody_elbow = (ChBodySceneNodeAuxRef*)addChBodySceneNode_Cascade_C(
&my_system, application.GetSceneManager(),
shape_elbow);
// Move the body as for global displacement/rotation
mrigidBody_elbow->GetBody()->ConcatenatePreTransformation(root_frame);
}
else GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_forearm;
if (mydoc.GetNamedShape(shape_forearm, "Assem10/Assem7" ))
{
// Add the shape to the Irrlicht system, to get also visualization.
mrigidBody_forearm = (ChBodySceneNodeAuxRef*)addChBodySceneNode_Cascade_C(
&my_system, application.GetSceneManager(),
shape_forearm);
// Move the body as for global displacement/rotation
mrigidBody_forearm->GetBody()->ConcatenatePreTransformation(root_frame);
}
else GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_wrist;
if (mydoc.GetNamedShape(shape_wrist, "Assem10/Assem6" ))
{
// Add the shape to the Irrlicht system, to get also visualization.
mrigidBody_wrist = (ChBodySceneNodeAuxRef*)addChBodySceneNode_Cascade_C(
&my_system, application.GetSceneManager(),
shape_wrist);
// Move the body as for global displacement/rotation
mrigidBody_wrist->GetBody()->ConcatenatePreTransformation(root_frame);
}
else GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_hand;
if (mydoc.GetNamedShape(shape_hand, "Assem10/Assem9" ))
{
// Add the shape to the Irrlicht system, to get also visualization.
mrigidBody_hand = (ChBodySceneNodeAuxRef*)addChBodySceneNode_Cascade_C(
&my_system, application.GetSceneManager(),
shape_hand);
//mrigidBody_hand->GetBody()->SetBodyFixed(true);
// Move the body as for global displacement/rotation
mrigidBody_hand->GetBody()->ConcatenatePreTransformation(root_frame);
}
else GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_cylinder;
if (mydoc.GetNamedShape(shape_cylinder, "Assem10/Assem3" ))
{
// Add the shape to the Irrlicht system, to get also visualization.
mrigidBody_cylinder = (ChBodySceneNodeAuxRef*)addChBodySceneNode_Cascade_C(
&my_system, application.GetSceneManager(),
shape_cylinder);
// Move the body as for global displacement/rotation
mrigidBody_cylinder->GetBody()->ConcatenatePreTransformation(root_frame);
}
else GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_rod;
if (mydoc.GetNamedShape(shape_rod, "Assem10/Assem2" ))
{
// Add the shape to the Irrlicht system, to get also visualization.
mrigidBody_rod = (ChBodySceneNodeAuxRef*)addChBodySceneNode_Cascade_C(
&my_system, application.GetSceneManager(),
shape_rod);
// Move the body as for global displacement/rotation
mrigidBody_rod->GetBody()->ConcatenatePreTransformation(root_frame);
}
else GetLog() << "Warning. Desired object not found in document \n";
}
else GetLog() << "Warning. Desired STEP file could not be opened/parsed \n";
if (!mrigidBody_base ||
!mrigidBody_turret ||
!mrigidBody_bicep ||
!mrigidBody_elbow ||
!mrigidBody_forearm ||
!mrigidBody_wrist ||
!mrigidBody_hand )
{
DLL_DeleteGlobals();
return 0;
}

Create joints between two parts. To understand where is the axis of the joint, we can exploit the fact that in the STEP file that we prepared for this demo, we inserted some objects called 'marker' and we placed them aligned to the shafts, so now we can fetch them and get their position/rotation.

Important! In the STEP file, some subassemblies have multiple instances of the marker, so there could be two or more shapes with the same name marker... how to select the desired one? The GetNamedShape() function has a feature for addressing this: you can use the # character followed by a number, so for example Assem10/Assem8/marker#2 means: get the 2nd instance of the shape called "marker" from the subassembly "Assem8" of assembly "Assem10".

TopoDS_Shape shape_marker;
ChFrame<> frame_marker_base_turret;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem8/marker#1" ))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_base_turret);
else GetLog() << "Warning. Desired marker not found in document \n";
// Transform the abs coordinates of the marker because everything was rotated/moved by 'root_frame' :
frame_marker_base_turret %= root_frame;
ChSharedPtr<ChLinkLockRevolute> my_link1(new ChLinkLockRevolute);
ChSharedBodyPtr mb1 = mrigidBody_base->GetBody();
ChSharedBodyPtr mb2 = mrigidBody_turret->GetBody();
my_link1->Initialize(mb1, mb2, frame_marker_base_turret.GetCoord() );
my_system.AddLink(my_link1);
ChFrame<> frame_marker_turret_bicep;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem4/marker#2" ))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_turret_bicep);
else GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_turret_bicep %= root_frame;
ChSharedPtr<ChLinkLockRevolute> my_link2(new ChLinkLockRevolute);
mb1 = mrigidBody_turret->GetBody();
mb2 = mrigidBody_bicep->GetBody();
my_link2->Initialize(mb1, mb2, frame_marker_turret_bicep.GetCoord() );
my_system.AddLink(my_link2);
ChFrame<> frame_marker_bicep_elbow;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem1/marker#2" ))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_bicep_elbow);
else GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_bicep_elbow %= root_frame;
ChSharedPtr<ChLinkLockRevolute> my_link3(new ChLinkLockRevolute);
mb1 = mrigidBody_bicep->GetBody();
mb2 = mrigidBody_elbow->GetBody();
my_link3->Initialize(mb1, mb2, frame_marker_bicep_elbow.GetCoord() );
my_system.AddLink(my_link3);
ChFrame<> frame_marker_elbow_forearm;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem5/marker#2" ))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_elbow_forearm);
else GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_elbow_forearm %= root_frame;
ChSharedPtr<ChLinkLockRevolute> my_link4(new ChLinkLockRevolute);
mb1 = mrigidBody_elbow->GetBody();
mb2 = mrigidBody_forearm->GetBody();
my_link4->Initialize(mb1, mb2, frame_marker_elbow_forearm.GetCoord() );
my_system.AddLink(my_link4);
ChFrame<> frame_marker_forearm_wrist;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem7/marker#2" ))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_forearm_wrist);
else GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_forearm_wrist %= root_frame;
ChSharedPtr<ChLinkLockRevolute> my_link5(new ChLinkLockRevolute);
mb1 = mrigidBody_forearm->GetBody();
mb2 = mrigidBody_wrist->GetBody();
my_link5->Initialize(mb1, mb2, frame_marker_forearm_wrist.GetCoord() );
my_system.AddLink(my_link5);
ChFrame<> frame_marker_wrist_hand;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem6/marker#2" ))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_wrist_hand);
else GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_wrist_hand %= root_frame;
ChSharedPtr<ChLinkLockRevolute> my_link6(new ChLinkLockRevolute);
mb1 = mrigidBody_wrist->GetBody();
mb2 = mrigidBody_hand->GetBody();
my_link6->Initialize(mb1, mb2, frame_marker_wrist_hand.GetCoord() );
my_system.AddLink(my_link6);
ChFrame<> frame_marker_turret_cylinder;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem4/marker#3" ))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_turret_cylinder);
else GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_turret_cylinder %= root_frame;
ChSharedPtr<ChLinkLockRevolute> my_link7(new ChLinkLockRevolute);
mb1 = mrigidBody_turret->GetBody();
mb2 = mrigidBody_cylinder->GetBody();
my_link7->Initialize(mb1, mb2, frame_marker_turret_cylinder.GetCoord() );
my_system.AddLink(my_link7);
ChFrame<> frame_marker_cylinder_rod;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem3/marker#2" ))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_cylinder_rod);
else GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_cylinder_rod %= root_frame;
ChSharedPtr<ChLinkLockCylindrical> my_link8(new ChLinkLockCylindrical);
mb1 = mrigidBody_cylinder->GetBody();
mb2 = mrigidBody_rod->GetBody();
my_link8->Initialize(mb1, mb2, frame_marker_cylinder_rod.GetCoord() );
my_system.AddLink(my_link8);
ChFrame<> frame_marker_rod_bicep;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem2/marker#2" ))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_rod_bicep);
else GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_rod_bicep %= root_frame;
ChSharedPtr<ChLinkLockCylindrical> my_link9(new ChLinkLockCylindrical);
mb1 = mrigidBody_rod->GetBody();
mb2 = mrigidBody_bicep->GetBody();
my_link9->Initialize(mb1, mb2, frame_marker_rod_bicep.GetCoord() );
my_system.AddLink(my_link9);

Add a couple of markers for the 'lock' constraint between the hand and the absolute reference: when we will move the marker in absolute reference, the hand will follow it.

This is a very simple way of performing the IK (Inverse Kinematics) of a robot, and it can be used to whatever type of robot, even parallel manipulators or complex kinematic chains, without the need of knowing the analytic expression of the IK.

ChSharedMarkerPtr my_marker_hand(new ChMarker);
ChSharedMarkerPtr my_marker_move(new ChMarker);
mrigidBody_hand->GetBody()->AddMarker(my_marker_hand);
mrigidBody_base->GetBody()->AddMarker(my_marker_move);
ChQuaternion<> rot_on_x; rot_on_x.Q_from_AngAxis(CH_C_PI/2, VECT_X);
ChFrame<> frame_marker_move = ChFrame<>(VNULL, rot_on_x) >> frame_marker_wrist_hand ;
my_marker_hand->Impose_Abs_Coord( frame_marker_wrist_hand.GetCoord() );
my_marker_move->Impose_Abs_Coord( frame_marker_move.GetCoord() );
ChSharedPtr<ChLinkLockLock> my_link_teacher(new ChLinkLockLock);
my_link_teacher->Initialize(my_marker_hand, my_marker_move);
my_system.AddLink(my_link_teacher);

Set motions for Z and Y coordinates of the 'my_link_teacher' marker, so that the hand will follow it. To do so, we create four segments of motion for Z coordinate and four for Y coordinate, we join them with ChFunction_Sequence and we repeat sequence by ChFunction_Repeat

ChFunction_ConstAcc* motlaw_z1 = new ChFunction_ConstAcc();
motlaw_z1->Set_h(-0.7);
motlaw_z1->Set_end(1);
ChFunction_Const* motlaw_z2 = new ChFunction_Const();
ChFunction_ConstAcc* motlaw_z3 = new ChFunction_ConstAcc();
motlaw_z3->Set_h( 0.7);
motlaw_z3->Set_end(1);
ChFunction_Const* motlaw_z4 = new ChFunction_Const();
ChFunction_Sequence* motlaw_z_seq = new ChFunction_Sequence();
motlaw_z_seq->InsertFunct(motlaw_z1, 1, 1, true);
motlaw_z_seq->InsertFunct(motlaw_z2, 1, 1, true); // true = force c0 continuity, traslating fx
motlaw_z_seq->InsertFunct(motlaw_z3, 1, 1, true);
motlaw_z_seq->InsertFunct(motlaw_z4, 1, 1, true);
ChFunction_Repeat* motlaw_z = new ChFunction_Repeat();
motlaw_z->Set_fa(motlaw_z_seq);
motlaw_z->Set_window_length(4);
ChFunction_Const* motlaw_y1 = new ChFunction_Const();
ChFunction_ConstAcc* motlaw_y2 = new ChFunction_ConstAcc();
motlaw_y2->Set_h(-0.6);
motlaw_y2->Set_end(1);
ChFunction_Const* motlaw_y3 = new ChFunction_Const();
ChFunction_ConstAcc* motlaw_y4 = new ChFunction_ConstAcc();
motlaw_y4->Set_h(0.6);
motlaw_y4->Set_end(1);
ChFunction_Sequence* motlaw_y_seq = new ChFunction_Sequence();
motlaw_y_seq->InsertFunct(motlaw_y1, 1, 1, true);
motlaw_y_seq->InsertFunct(motlaw_y2, 1, 1, true); // true = force c0 continuity, traslating fx
motlaw_y_seq->InsertFunct(motlaw_y3, 1, 1, true);
motlaw_y_seq->InsertFunct(motlaw_y4, 1, 1, true);
ChFunction_Repeat* motlaw_y = new ChFunction_Repeat();
motlaw_y->Set_fa(motlaw_y_seq);
motlaw_y->Set_window_length(4);
my_marker_move->SetMotion_Z(motlaw_z);
my_marker_move->SetMotion_Y(motlaw_y);
// Create a large cube as a floor.
ChBodySceneNode* mfloor = (ChBodySceneNode*)addChBodySceneNode_easyBox(
&my_system, application.GetSceneManager(),
1000.0,
ChVector<>(0,-0.6,0),
ChQuaternion<>(1,0,0,0),
ChVector<>(20,1,20) );
mfloor->GetBody()->SetBodyFixed(true);
mfloor->GetBody()->SetCollide(true);
video::ITexture* cubeMap = application.GetVideoDriver()->getTexture("../data/blu.png");
mfloor->setMaterialTexture(0, cubeMap);

Modify the settings of the solver. By default, the solver might not have sufficient precision to keep the robot joints 'mounted'. Expecially, the SOR, SSOR and other fixed point methods cannot simulate well this robot problem because the mass of the last body in the kinematic chain, i.e. the hand, is very low when compared to other bodies, so the convergence of the solver would be bad when 'pulling the hand' as in this 'teaching mode' IK. So switch to a more precise solver; this SOLVER_ITERATIVE_MINRES is fast and precise (although it is not fit for frictional collisions):

my_system.SetLcpSolverType(ChSystem::SOLVER_MINRES);

Now, finally, run the loop of the simulator: here are snapshots from the real-time simulator:

Here is the full source code:

// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Show how to use the OpenCASCADE features
// implemented in the unit_CASCADE:
//
// - load a 3D model saved in STEP format from a CAD
// - select some sub assemblies from the STEP model
// - make Chrono::Engine objects out of those parts
// =============================================================================
#include "chrono/core/ChRealtimeStep.h"
#include "chrono/physics/ChSystemNSC.h"
#include "chrono/physics/ChBodyEasy.h"
#include "chrono_cascade/ChBodyEasyCascade.h"
#include "chrono_cascade/ChCascadeDoc.h"
#include "chrono_cascade/ChCascadeShapeAsset.h"
#include "chrono_irrlicht/ChIrrApp.h"
#include "chrono/solver/ChSolverADMM.h"
#include "chrono/assets/ChColorAsset.h"
// Use the namespace of Chrono
using namespace chrono;
using namespace chrono::irrlicht;
// Use the main namespaces of Irlicht
using namespace irr;
using namespace core;
using namespace scene;
using namespace video;
using namespace io;
using namespace gui;
// Use the namespace with OpenCascade stuff
using namespace cascade;
//
// This is the program which is executed
//
int main(int argc, char* argv[]) {
// 1- Create a ChronoENGINE physical system: all bodies and constraints
// will be handled by this ChSystemNSC object.
ChSystemNSC my_system;
// Create a surface material to be used for collisions, if any
auto mysurfmaterial = chrono_types::make_shared<ChMaterialSurfaceNSC>();
mysurfmaterial->SetFriction(0.3f);
mysurfmaterial->SetRestitution(0);
// Create the Irrlicht visualization (open the Irrlicht device,
// bind a simple user interface, etc. etc.)
ChIrrApp application(&my_system, L"Load a robot model from STEP file", core::dimension2d<u32>(800, 600));
application.AddTypicalLogo();
application.AddTypicalSky();
application.AddTypicalLights(core::vector3df(30, 100, 30), core::vector3df(30, -80, -30), 200, 130);
application.AddTypicalCamera(core::vector3df(0.2f, 1.6f, 3.5f), core::vector3df(0.0f, 1.0f, 0.0f));
//
// Load a STEP file, containing a mechanism. The demo STEP file has been
// created using a 3D CAD (in this case, SolidEdge v.18).
//
// Create the ChCascadeDoc, a container that loads the STEP model
// and manages its subassemblies
ChCascadeDoc mydoc;
// load the STEP model using this command:
bool load_ok = mydoc.Load_STEP(GetChronoDataFile("/cascade/IRB7600_23_500_m2000_rev1_01_decorated.stp").c_str());
// print the contained shapes
mydoc.Dump(GetLog());
//
// Retrieve some sub shapes from the loaded model, using
// the GetNamedShape() function, that can use path/subpath/subsubpath/part
// syntax and * or ? wldcards, etc.
//
std::shared_ptr<ChBodyEasyCascade> mrigidBody_base;
std::shared_ptr<ChBodyEasyCascade> mrigidBody_turret;
std::shared_ptr<ChBodyEasyCascade> mrigidBody_bicep;
std::shared_ptr<ChBodyEasyCascade> mrigidBody_elbow;
std::shared_ptr<ChBodyEasyCascade> mrigidBody_forearm;
std::shared_ptr<ChBodyEasyCascade> mrigidBody_wrist;
std::shared_ptr<ChBodyEasyCascade> mrigidBody_hand;
std::shared_ptr<ChBodyEasyCascade> mrigidBody_cylinder;
std::shared_ptr<ChBodyEasyCascade> mrigidBody_rod;
// Note, In most CADs the Y axis is horizontal, but we want it vertical.
// So define a root transformation for rotating all the imported objects.
ChQuaternion<> rotation1;
rotation1.Q_from_AngAxis(-CH_C_PI / 2, VECT_X); // 1: rotate 90� on X axis
ChQuaternion<> rotation2;
rotation2.Q_from_AngAxis(CH_C_PI, VECT_Y); // 2: rotate 180� on vertical Y axis
ChQuaternion<> tot_rotation = rotation2 % rotation1; // rotate on 1 then on 2, using quaternion product
ChFrameMoving<> root_frame(ChVector<>(0, 0, 0), tot_rotation);
if (load_ok) {
TopoDS_Shape shape_base;
if (mydoc.GetNamedShape(shape_base, "Assem10/Assem8")) {
auto mbody = chrono_types::make_shared<ChBodyEasyCascade>(shape_base, 1000, true, false);
mrigidBody_base = mbody;
my_system.Add(mrigidBody_base);
// The base is fixed to the ground
mrigidBody_base->SetBodyFixed(true);
mrigidBody_base->ConcatenatePreTransformation(root_frame);
} else
GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_turret;
if (mydoc.GetNamedShape(shape_turret, "Assem10/Assem4")) {
auto mbody = chrono_types::make_shared<ChBodyEasyCascade>(shape_turret, 1000, true, false);
mrigidBody_turret = mbody;
my_system.Add(mrigidBody_turret);
// Move the body as for global displacement/rotation
mrigidBody_turret->ConcatenatePreTransformation(root_frame);
} else
GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_bicep;
if (mydoc.GetNamedShape(shape_bicep, "Assem10/Assem1")) {
auto mbody = chrono_types::make_shared<ChBodyEasyCascade>(shape_bicep, 1000, true, false);
mrigidBody_bicep = mbody;
my_system.Add(mrigidBody_bicep);
// Move the body as for global displacement/rotation
mrigidBody_bicep->ConcatenatePreTransformation(root_frame);
} else
GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_elbow;
if (mydoc.GetNamedShape(shape_elbow, "Assem10/Assem5")) {
auto mbody = chrono_types::make_shared<ChBodyEasyCascade>(shape_elbow, 1000, true, false);
mrigidBody_elbow = mbody;
my_system.Add(mrigidBody_elbow);
// Move the body as for global displacement/rotation
mrigidBody_elbow->ConcatenatePreTransformation(root_frame);
} else
GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_forearm;
if (mydoc.GetNamedShape(shape_forearm, "Assem10/Assem7")) {
auto mbody = chrono_types::make_shared<ChBodyEasyCascade>(shape_forearm, 1000, true, false);
mrigidBody_forearm = mbody;
my_system.Add(mrigidBody_forearm);
// Move the body as for global displacement/rotation
mrigidBody_forearm->ConcatenatePreTransformation(root_frame);
} else
GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_wrist;
if (mydoc.GetNamedShape(shape_wrist, "Assem10/Assem6")) {
auto mbody = chrono_types::make_shared<ChBodyEasyCascade>(shape_wrist, 1000, true, false);
mrigidBody_wrist = mbody;
my_system.Add(mrigidBody_wrist);
// Move the body as for global displacement/rotation
mrigidBody_wrist->ConcatenatePreTransformation(root_frame);
} else
GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_hand;
if (mydoc.GetNamedShape(shape_hand, "Assem10/Assem9")) {
auto mbody = chrono_types::make_shared<ChBodyEasyCascade>(shape_hand, 1000, true, false);
mrigidBody_hand = mbody;
my_system.Add(mrigidBody_hand);
// Move the body as for global displacement/rotation
mrigidBody_hand->ConcatenatePreTransformation(root_frame);
// also create a collision shape attached to the hand:
auto mcube = chrono_types::make_shared<ChBodyEasyBox>(0.2, 0.08, 0.08, 1000, true, true, mysurfmaterial);
mcube->SetCoord(ChCoordsys<>(ChVector<>(0.1,0,0)) >> mrigidBody_hand->GetCoord());
my_system.Add(mcube);
auto mcubelink = chrono_types::make_shared<ChLinkLockLock>();
mcubelink->Initialize(mcube, mrigidBody_hand, mcube->GetCoord());
my_system.Add(mcubelink);
} else
GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_cylinder;
if (mydoc.GetNamedShape(shape_cylinder, "Assem10/Assem3")) {
auto mbody = chrono_types::make_shared<ChBodyEasyCascade>(shape_cylinder, 1000, true, false);
mrigidBody_cylinder = mbody;
my_system.Add(mrigidBody_cylinder);
// Move the body as for global displacement/rotation
mrigidBody_cylinder->ConcatenatePreTransformation(root_frame);
} else
GetLog() << "Warning. Desired object not found in document \n";
TopoDS_Shape shape_rod;
if (mydoc.GetNamedShape(shape_rod, "Assem10/Assem2")) {
auto mbody = chrono_types::make_shared<ChBodyEasyCascade>(shape_rod, 1000, true, false);
mrigidBody_rod = mbody;
my_system.Add(mrigidBody_rod);
// Move the body as for global displacement/rotation
mrigidBody_rod->ConcatenatePreTransformation(root_frame);
} else
GetLog() << "Warning. Desired object not found in document \n";
} else
GetLog() << "Warning. Desired STEP file could not be opened/parsed \n";
if (!mrigidBody_base || !mrigidBody_turret || !mrigidBody_bicep || !mrigidBody_elbow || !mrigidBody_forearm ||
!mrigidBody_wrist || !mrigidBody_hand) {
return 1;
}
// add color to created bodies
for (auto mbody : my_system.Get_bodylist()) {
auto mcol = chrono_types::make_shared<ChColorAsset>(1.0f,0.5f,0.0f);
mbody->AddAsset(mcol);
}
// Create joints between two parts.
// To understand where is the axis of the joint, we can exploit the fact
// that in the STEP file that we prepared for this demo, we inserted some
// objects called 'marker' and we placed them aligned to the shafts, so now
// we can fetch them and get their position/rotation.
TopoDS_Shape shape_marker;
ChFrame<> frame_marker_base_turret;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem8/marker#1"))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_base_turret);
else
GetLog() << "Warning. Desired marker not found in document \n";
// Transform the abs coordinates of the marker because everything was rotated/moved by 'root_frame' :
frame_marker_base_turret %= root_frame;
std::shared_ptr<ChLinkLockRevolute> my_link1(new ChLinkLockRevolute);
my_link1->Initialize(mrigidBody_base, mrigidBody_turret, frame_marker_base_turret.GetCoord());
my_system.AddLink(my_link1);
ChFrame<> frame_marker_turret_bicep;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem4/marker#2"))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_turret_bicep);
else
GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_turret_bicep %= root_frame;
std::shared_ptr<ChLinkLockRevolute> my_link2(new ChLinkLockRevolute);
my_link2->Initialize(mrigidBody_turret, mrigidBody_bicep, frame_marker_turret_bicep.GetCoord());
my_system.AddLink(my_link2);
ChFrame<> frame_marker_bicep_elbow;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem1/marker#2"))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_bicep_elbow);
else
GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_bicep_elbow %= root_frame;
std::shared_ptr<ChLinkLockRevolute> my_link3(new ChLinkLockRevolute);
my_link3->Initialize(mrigidBody_bicep, mrigidBody_elbow, frame_marker_bicep_elbow.GetCoord());
my_system.AddLink(my_link3);
ChFrame<> frame_marker_elbow_forearm;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem5/marker#2"))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_elbow_forearm);
else
GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_elbow_forearm %= root_frame;
std::shared_ptr<ChLinkLockRevolute> my_link4(new ChLinkLockRevolute);
my_link4->Initialize(mrigidBody_elbow, mrigidBody_forearm, frame_marker_elbow_forearm.GetCoord());
my_system.AddLink(my_link4);
ChFrame<> frame_marker_forearm_wrist;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem7/marker#2"))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_forearm_wrist);
else
GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_forearm_wrist %= root_frame;
std::shared_ptr<ChLinkLockRevolute> my_link5(new ChLinkLockRevolute);
my_link5->Initialize(mrigidBody_forearm, mrigidBody_wrist, frame_marker_forearm_wrist.GetCoord());
my_system.AddLink(my_link5);
ChFrame<> frame_marker_wrist_hand;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem6/marker#2"))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_wrist_hand);
else
GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_wrist_hand %= root_frame;
std::shared_ptr<ChLinkLockRevolute> my_link6(new ChLinkLockRevolute);
my_link6->Initialize(mrigidBody_wrist, mrigidBody_hand, frame_marker_wrist_hand.GetCoord());
my_system.AddLink(my_link6);
ChFrame<> frame_marker_turret_cylinder;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem4/marker#3"))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_turret_cylinder);
else
GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_turret_cylinder %= root_frame;
std::shared_ptr<ChLinkLockRevolute> my_link7(new ChLinkLockRevolute);
my_link7->Initialize(mrigidBody_turret, mrigidBody_cylinder, frame_marker_turret_cylinder.GetCoord());
my_system.AddLink(my_link7);
ChFrame<> frame_marker_cylinder_rod;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem3/marker#2"))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_cylinder_rod);
else
GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_cylinder_rod %= root_frame;
std::shared_ptr<ChLinkLockCylindrical> my_link8(new ChLinkLockCylindrical);
my_link8->Initialize(mrigidBody_cylinder, mrigidBody_rod, frame_marker_cylinder_rod.GetCoord());
my_system.AddLink(my_link8);
ChFrame<> frame_marker_rod_bicep;
if (mydoc.GetNamedShape(shape_marker, "Assem10/Assem2/marker#2"))
ChCascadeDoc::FromCascadeToChrono(shape_marker.Location(), frame_marker_rod_bicep);
else
GetLog() << "Warning. Desired marker not found in document \n";
frame_marker_rod_bicep %= root_frame;
std::shared_ptr<ChLinkLockCylindrical> my_link9(new ChLinkLockCylindrical);
my_link9->Initialize(mrigidBody_rod, mrigidBody_bicep, frame_marker_rod_bicep.GetCoord());
my_system.AddLink(my_link9);
// Add a couple of markers for the 'lock' constraint between the hand and the
// absolute reference: when we will move the marker in absolute reference, the
// hand will follow it.
std::shared_ptr<ChMarker> my_marker_hand(new ChMarker);
std::shared_ptr<ChMarker> my_marker_move(new ChMarker);
mrigidBody_hand->AddMarker(my_marker_hand);
mrigidBody_base->AddMarker(my_marker_move);
ChQuaternion<> rot_on_x;
rot_on_x.Q_from_AngAxis(CH_C_PI / 2, VECT_X);
ChFrame<> frame_marker_move = ChFrame<>(VNULL, rot_on_x) >> frame_marker_wrist_hand;
my_marker_hand->Impose_Abs_Coord(frame_marker_wrist_hand.GetCoord());
my_marker_move->Impose_Abs_Coord(frame_marker_move.GetCoord());
std::shared_ptr<ChLinkLockLock> my_link_teacher(new ChLinkLockLock);
my_link_teacher->Initialize(my_marker_hand, my_marker_move);
my_system.AddLink(my_link_teacher);
// Set motions for Z and Y coordinates of the 'my_link_teacher' marker,
// so that the hand will follow it. To do so, we create four segments of
// motion for Z coordinate and four for Y coordinate, we join them with
// ChFunction_Sequence and we repeat sequence by ChFunction_Repeat
std::shared_ptr<ChFunction_ConstAcc> motlaw_z1(new ChFunction_ConstAcc);
motlaw_z1->Set_h(-0.7);
motlaw_z1->Set_end(1);
std::shared_ptr<ChFunction_Const> motlaw_z2(new ChFunction_Const);
std::shared_ptr<ChFunction_ConstAcc> motlaw_z3(new ChFunction_ConstAcc);
motlaw_z3->Set_h(0.7);
motlaw_z3->Set_end(1);
std::shared_ptr<ChFunction_Const> motlaw_z4(new ChFunction_Const);
std::shared_ptr<ChFunction_Sequence> motlaw_z_seq(new ChFunction_Sequence);
motlaw_z_seq->InsertFunct(motlaw_z1, 1, 1, true);
motlaw_z_seq->InsertFunct(motlaw_z2, 1, 1, true); // true = force c0 continuity, traslating fx
motlaw_z_seq->InsertFunct(motlaw_z3, 1, 1, true);
motlaw_z_seq->InsertFunct(motlaw_z4, 1, 1, true);
std::shared_ptr<ChFunction_Repeat> motlaw_z(new ChFunction_Repeat);
motlaw_z->Set_fa(motlaw_z_seq);
motlaw_z->Set_window_length(4);
std::shared_ptr<ChFunction_Const> motlaw_y1(new ChFunction_Const);
std::shared_ptr<ChFunction_ConstAcc> motlaw_y2(new ChFunction_ConstAcc);
motlaw_y2->Set_h(-0.6);
motlaw_y2->Set_end(1);
std::shared_ptr<ChFunction_Const> motlaw_y3(new ChFunction_Const);
std::shared_ptr<ChFunction_ConstAcc> motlaw_y4(new ChFunction_ConstAcc);
motlaw_y4->Set_h(0.6);
motlaw_y4->Set_end(1);
std::shared_ptr<ChFunction_Sequence> motlaw_y_seq(new ChFunction_Sequence);
motlaw_y_seq->InsertFunct(motlaw_y1, 1, 1, true);
motlaw_y_seq->InsertFunct(motlaw_y2, 1, 1, true); // true = force c0 continuity, traslating fx
motlaw_y_seq->InsertFunct(motlaw_y3, 1, 1, true);
motlaw_y_seq->InsertFunct(motlaw_y4, 1, 1, true);
std::shared_ptr<ChFunction_Repeat> motlaw_y(new ChFunction_Repeat);
motlaw_y->Set_fa(motlaw_y_seq);
motlaw_y->Set_window_length(4);
my_marker_move->SetMotion_Z(motlaw_z);
my_marker_move->SetMotion_Y(motlaw_y);
// Create a large cube as a floor.
std::shared_ptr<ChBodyEasyBox> mfloor(new ChBodyEasyBox(8, 1, 8, 1000, true, true, mysurfmaterial));
mfloor->SetPos(ChVector<>(0, -0.5, 0));
my_system.Add(mfloor);
mfloor->SetBodyFixed(true);
// Create a stack of boxes to be impacted
if (true) {
double brick_h = 0.3;
for (int ix = 0; ix < 3; ++ix)
for (int ib = 0; ib < 6; ++ib) {
std::shared_ptr<ChBodyEasyBox> mcube(new ChBodyEasyBox(0.4, brick_h, 0.4, 1000, true, true, mysurfmaterial));
mcube->SetPos(ChVector<>(-1.4, (0.5*brick_h)+ib*brick_h, -0.4 -0.5*ix));
mcube->SetRot(Q_from_AngAxis(ib*0.1,VECT_Y));
my_system.Add(mcube);
auto mcol = chrono_types::make_shared<ChColorAsset>(0.5f+float(0.5*ChRandom()), 0.5f+float(0.5*ChRandom()), 0.5f+float(0.5*ChRandom()));
mcube->AddAsset(mcol);
}
}
std::shared_ptr<ChTexture> mtexture(new ChTexture(GetChronoDataFile("textures/blue.png").c_str()));
mfloor->AddAsset(mtexture);
// Use this function for adding a ChIrrNodeAsset to all items
// Otherwise use application.AssetBind(myitem); on a per-item basis.
application.AssetBindAll();
// Use this function for 'converting' assets into Irrlicht meshes
application.AssetUpdateAll();
//
// THE SOFT-REAL-TIME CYCLE, SHOWING THE SIMULATION
//
// Modify the settings of the solver.
// By default, the solver might not have sufficient precision to keep the
// robot joints 'mounted'. Expecially, the SOR, SSOR and other fixed point methods
// cannot simulate well this robot problem because the mass of the last body in the
// kinematic chain, i.e. the hand, is very low when compared to other bodies, so
// the convergence of the solver would be bad when 'pulling the hand' as in this
// 'teaching mode' IK.
// So switch to a more precise solver, ex. BARZILAIBORWEIN
my_system.SetSolverMaxIterations(200);
/*
// Alternative: the ADMM solver offers higher precision and it can also support FEA + nonsmooth contacts
auto solver = chrono_types::make_shared<ChSolverADMM>(); //faster, if MKL enabled: chrono_types::make_shared<ChSolverPardisoMKL>());
solver->EnableWarmStart(true);
solver->SetMaxIterations(60);
solver->SetRho(1);
solver->SetSigma(1e-8);
solver->SetStepAdjustPolicy(ChSolverADMM::AdmmStepType::BALANCED_UNSCALED);
my_system.SetSolver(solver);
*/
//
// THE SOFT-REAL-TIME CYCLE, SHOWING THE SIMULATION
//
application.SetTimestep(0.01);
application.SetTryRealtime(true);
while (application.GetDevice()->run()) {
// Irrlicht must prepare frame to draw
application.BeginScene(true, true, video::SColor(255, 140, 161, 192));
// .. draw solid 3D items (boxes, cylinders, shapes) belonging to Irrlicht scene, if any
application.DrawAll();
// .. perform timestep simulation
application.DoStep();
// .. plot something on realtime view
tools::drawChFunction(application.GetDevice(), motlaw_z.get(), 0, 10, -0.9, 0.2, 10, 400, 300, 80);
tools::drawChFunction(application.GetDevice(), motlaw_y.get(), 0, 10, -0.9, 0.2, 10, 500, 300, 80);
application.EndScene();
}
return 0;
}
Constant function: y = C
Definition: ChFunction_Const.h:26
std::string GetChronoDataFile(const std::string &filename)
Obtain the complete path to the specified filename, given relative to the Chrono data directory (thre...
Definition: ChGlobal.cpp:95
void Add(std::shared_ptr< ChPhysicsItem > item)
Attach an arbitrary ChPhysicsItem (e.g.
Definition: ChSystem.cpp:170
static void SetDefaultSuggestedMargin(double mmargin)
Using this function BEFORE you start creating collision shapes, it will make all following collision ...
Definition: ChCollisionModel.cpp:44
Class for quick creation of rigid bodies with a box shape.
Definition: ChBodyEasy.h:135
COORDSYS:
Definition: ChCoordsys.h:38
ChLog & GetLog()
Global function to get the current ChLog object.
Definition: ChLog.cpp:39
void SetTimestep(double val)
Set/Get the time step for time integration.
Definition: ChIrrAppInterface.cpp:552
virtual void AddLink(std::shared_ptr< ChLinkBase > link)
Attach a link to the underlying assembly.
Definition: ChSystem.cpp:155
Repeat function: y = __/__/__/
Definition: ChFunction_Repeat.h:36
ChApiIrr void add_typical_Sky(irr::IrrlichtDevice *device, bool y_up=true, const std::string &mtexturedir=GetChronoDataFile("skybox/"))
A very basic and simple function which is just a shortcut to avoid lot of typing when someone wants t...
ChFrame: a class for coordinate systems in 3D space.
Definition: ChFrame.h:42
Class to add some GUI to Irrlicht+ChronoEngine applications.
Definition: ChIrrApp.h:29
ChQuaternion< double > Q_from_AngAxis(double angle, const ChVector< double > &axis)
Get the quaternion from an angle of rotation and an axis, defined in abs coords.
Definition: ChQuaternion.cpp:100
virtual void EndScene()
Call this to end the scene draw at the end of each animation frame.
Definition: ChIrrAppInterface.cpp:627
const std::vector< std::shared_ptr< ChBody > > & Get_bodylist() const
Get the list of bodies.
Definition: ChSystem.h:318
Definition of general purpose 3d vector variables, such as points in 3D.
Definition: ChVector.h:35
virtual void DoStep()
Call this function inside a loop such as.
Definition: ChIrrAppInterface.cpp:637
Base class for assets that define basic textures.
Definition: ChTexture.h:25
void SetSolverType(ChSolver::Type type)
Choose the solver type, to be used for the simultaneous solution of the constraints in dynamical simu...
Definition: ChSystem.cpp:238
Markers are auxiliary reference frames which belong to rigid bodies and move together with them.
Definition: ChMarker.h:38
void SetSolverMaxIterations(int max_iters)
Set the maximum number of iterations, if using an iterative solver.
Definition: ChSystem.cpp:212
ChFrameMoving: a class for coordinate systems in 3D space.
Definition: ChFrameMoving.h:38
double ChRandom()
Returns random value in (0..1) interval with Park-Miller method.
Definition: ChMathematics.cpp:53
ChCoordsys< Real > & GetCoord()
Return both current rotation and translation as a coordsystem object, with vector and quaternion.
Definition: ChFrame.h:201
Class defining quaternion objects, that is four-dimensional numbers, also known as Euler parameters.
Definition: ChQuaternion.h:45
static void SetDefaultSuggestedEnvelope(double menv)
Using this function BEFORE you start creating collision shapes, it will make all following collision ...
Definition: ChCollisionModel.cpp:40
Constant acceleration function:
Definition: ChFunction_ConstAcc.h:31
virtual void BeginScene(bool backBuffer=true, bool zBuffer=true, irr::video::SColor color=irr::video::SColor(255, 0, 0, 0))
Call this to clean the canvas at the beginning of each animation frame.
Definition: ChIrrAppInterface.cpp:610
ChApiIrr void add_typical_Lights(irr::IrrlichtDevice *device, irr::core::vector3df pos1=irr::core::vector3df(30.f, 100.f, 30.f), irr::core::vector3df pos2=irr::core::vector3df(30.f, 80.f, -30.f), double rad1=290, double rad2=190, irr::video::SColorf col1=irr::video::SColorf(0.7f, 0.7f, 0.7f, 1.0f), irr::video::SColorf col2=irr::video::SColorf(0.7f, 0.8f, 0.8f, 1.0f))
A very basic and simple function which is just a shortcut to avoid lot of typing when someone wants t...
Main namespace for the Chrono package.
Definition: ChAsset.cpp:18
void Q_from_AngAxis(Real angle, const ChVector< Real > &axis)
Set the quaternion from an angle of rotation and an axis, defined in absolute coords.
Definition: ChQuaternion.h:1129
virtual void DrawAll()
Call this function inside a loop such as.
Definition: ChIrrAppInterface.cpp:750
ChApiIrr void add_typical_Camera(irr::IrrlichtDevice *device, irr::core::vector3df pos=irr::core::vector3df(0, 0, -8), irr::core::vector3df targ=irr::core::vector3df(0, 0, 0), bool y_up=true)
A very basic and simple function which is just a shortcut to avoid lot of typing when someone wants t...
Class for a physical system in which contact is modeled using a non-smooth (complementarity-based) me...
Definition: ChSystemNSC.h:29
void SetTryRealtime(bool val)
If enabled, the function DoStep() will enforce soft real-time, by spinning in place until simulation ...
Definition: ChIrrAppInterface.h:98
Sequence function: y = sequence_of_functions(f1(y), f2(y), f3(y)) All other function types can be ins...
Definition: ChFunction_Sequence.h:68