Hello,
I am trying to simulate a robotic movement where the robot has two arms, in
the initial state the end effectors are fixed. Meaning the robotic
structure creates a closed loop and an over constrained mechanism. But this
results in the simulation in a oscillating movement, see attached file,
which should not be present.
The robots body is initialy fixed, but this is deactivated shortly after
the start.
Is there a possibility to avoid these oscillations i.e. solver or stepper
settings?
-Jakob
--
You received this message because you are subscribed to the Google Groups
"ProjectChrono" group.
To unsubscribe from this group and stop receiving emails from it, send an email
to [email protected].
To view this discussion visit
https://groups.google.com/d/msgid/projectchrono/5e4e338d-3d1b-4a4e-9d46-0ef151b6e93cn%40googlegroups.com.
#include "chrono/physics/ChSystemNSC.h"
#include "chrono/physics/ChSystemSMC.h"
#include "chrono/physics/ChBodyEasy.h"
#include "chrono/physics/ChBody.h"
#include "chrono/physics/ChLinkMate.h"
#include "chrono/physics/ChLinkRSDA.h"
#include "chrono/physics/ChLinkMotorRotationSpeed.h"
#include "chrono/physics/ChLinkMotorLinearPosition.h"
#include "chrono/physics/ChLinkMotorRotationAngle.h"
#include "chrono/physics/ChLinkMotorLinearForce.h"
#include <vector>
#include <cstdio>
#include <cmath>
#include <string>
#include "chrono/assets/ChVisualShapeTriangleMesh.h"
#include "chrono_irrlicht/ChVisualSystemIrrlicht.h"
#include "chrono/core/ChRealtimeStep.h"
#include <chrono/solver/ChSolverBB.h>
#include "chrono/geometry/ChTriangleMeshConnected.h"
using namespace chrono;
using namespace chrono::irrlicht;
using namespace irr;
ChVisualSystem::Type vis_type = ChVisualSystem::Type::IRRLICHT;
ChContactMethod contact_method = ChContactMethod::NSC;
ChCollisionSystem::Type collision_type = ChCollisionSystem::Type::BULLET;
double time_step = 0.0001;
void test_system_rigid_body_2() {
time_step = 0.001;
ChSystemNSC sys;
sys.SetCollisionSystemType(collision_type);
sys.SetGravitationalAcceleration(ChVector3d(0, -9.81, 0));
auto material = chrono_types::make_shared<ChContactMaterialNSC>();
//
double r_u = 0.05;
double l = 0.2;
double l2 = 0.3;
double l_body = 0.5;
auto frame_1 = ChFrame(VNULL, QuatFromAngleZ(CH_PI * 1 / 3));
auto frame_joint_r_1 = frame_1 * ChFrame(ChVector3d(0, 0, l_body / 2),
QuatFromAngleX(-CH_PI_2 + 0) * QuatFromAngleZ(CH_PI * 0));
auto frame_joint_l_1 = frame_1 * ChFrame(ChVector3d(0, 0, -l_body / 2),
QuatFromAngleX(-CH_PI_2 + -CH_PI_3) * QuatFromAngleZ(CH_PI * 0));
auto frame_joint_r_2 = frame_joint_r_1 * ChFrame(ChVector3d(0, 0,
l_body)) * ChFrame(VNULL, Q_ROTATE_Z_TO_Y * QuatFromAngleZ(CH_PI) *
QuatFromAngleZ(CH_PI * 0));
auto frame_joint_l_2 = frame_joint_l_1 * ChFrame(ChVector3d(0, 0,
l_body)) * ChFrame(VNULL, Q_ROTATE_Z_TO_Y * QuatFromAngleZ(CH_PI) *
QuatFromAngleZ(CH_PI * 0));
auto frame_joint_r_3 = frame_joint_r_2 * ChFrame(ChVector3d(0, l_body,
0), QuatFromAngleZ(CH_PI * 0));
auto frame_joint_l_3 = frame_joint_l_2 * ChFrame(ChVector3d(0, l_body,
0), QuatFromAngleZ(CH_PI * 0));
auto frame_joint_r_4 = frame_joint_r_3 * ChFrame(ChVector3d(0, l_body,
0), Q_ROTATE_Z_TO_Y);
auto frame_joint_l_4 = frame_joint_l_3 * ChFrame(ChVector3d(0, l_body,
0), Q_ROTATE_Z_TO_Y);
auto frame_joint_r_5 = frame_joint_r_4 * ChFrame(ChVector3d(0, 0, r_u));
auto frame_joint_l_5 = frame_joint_l_4 * ChFrame(ChVector3d(0, 0, r_u));
auto frame_joint_r_6 = frame_joint_r_5 * ChFrame(VNULL,
QuatFromAngleY(CH_PI * 0) * QuatFromAngleX(CH_PI * 0)) * ChFrame(ChVector3d(0,
0, r_u));
auto frame_joint_l_6 = frame_joint_l_5 * ChFrame(VNULL,
QuatFromAngleY(CH_PI * 0) * QuatFromAngleX(CH_PI * 0)) * ChFrame(ChVector3d(0,
0, r_u));
auto frame_gripper_1 = frame_joint_r_6 * ChFrame(ChVector3d(0, 0, l2));
auto frame_gripper_2 = frame_joint_l_6 * ChFrame(ChVector3d(0, 0, l2));
auto body_swing = chrono_types::make_shared<ChBodyEasyBox>(0.1, 0.1,
l_body, 1000, material); body_swing->SetFixed(false);
body_swing->SetPos(frame_1.GetPos()); body_swing->SetRot(frame_1.GetRot());
sys.AddBody(body_swing); body_swing->EnableCollision(false);
// body_layer_2
auto body_swing_r_1 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
0.1, l_body, 1000, material); body_swing_r_1->SetFixed(false);
body_swing_r_1->SetPos(frame_joint_r_1 * ChVector3d(0, 0, l_body / 2));
body_swing_r_1->SetRot(frame_joint_r_1.GetRot()); sys.AddBody(body_swing_r_1);
body_swing_r_1->EnableCollision(false);
auto body_swing_l_1 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
0.1, l_body, 1000, material); body_swing_l_1->SetFixed(false);
body_swing_l_1->SetPos(frame_joint_l_1 * ChVector3d(0, 0, l_body / 2));
body_swing_l_1->SetRot(frame_joint_l_1.GetRot()); sys.AddBody(body_swing_l_1);
body_swing_l_1->EnableCollision(false);
//layer 3
auto body_swing_r_2 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
l_body, 0.1, 1000, material); body_swing_r_2->SetFixed(false);
body_swing_r_2->SetPos(frame_joint_r_2 * ChVector3d(0, l_body / 2, 0));
body_swing_r_2->SetRot(frame_joint_r_2.GetRot()); sys.AddBody(body_swing_r_2);
body_swing_r_2->EnableCollision(false);
auto body_swing_l_2 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
l_body, 0.1, 1000, material); body_swing_l_2->SetFixed(false);
body_swing_l_2->SetPos(frame_joint_l_2 * ChVector3d(0, l_body / 2, 0));
body_swing_l_2->SetRot(frame_joint_l_2.GetRot()); sys.AddBody(body_swing_l_2);
body_swing_l_2->EnableCollision(false);
//layer 4
auto body_swing_r_3 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
l_body, 0.1, 1000, material); body_swing_r_3->SetFixed(false);
body_swing_r_3->SetPos(frame_joint_r_3 * ChVector3d(0, l_body / 2, 0));
body_swing_r_3->SetRot(frame_joint_r_3.GetRot()); sys.AddBody(body_swing_r_3);
body_swing_r_3->EnableCollision(false);
auto body_swing_l_3 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
l_body, 0.1, 1000, material); body_swing_l_3->SetFixed(false);
body_swing_l_3->SetPos(frame_joint_l_3 * ChVector3d(0, l_body / 2, 0));
body_swing_l_3->SetRot(frame_joint_l_3.GetRot()); sys.AddBody(body_swing_l_3);
body_swing_l_3->EnableCollision(false);
// layer 5
auto body_swing_r_4 =
chrono_types::make_shared<ChBodyEasyCylinder>(ChAxis::Z, 0.1, l2, 1000,
material); body_swing_r_4->SetFixed(false);
body_swing_r_4->SetPos(frame_joint_r_6 * ChVector3d(0, 0, l2 / 2));
body_swing_r_4->SetRot(frame_joint_r_6.GetRot()); sys.AddBody(body_swing_r_4);
body_swing_r_4->EnableCollision(false);
auto body_swing_l_4 = chrono_types::make_shared<ChBodyEasyCylinder
>(ChAxis::Z, 0.1, l2, 1000, material); body_swing_l_4->SetFixed(false);
body_swing_l_4->SetPos(frame_joint_l_6 * ChVector3d(0, 0, l2 / 2));
body_swing_l_4->SetRot(frame_joint_l_6.GetRot()); sys.AddBody(body_swing_l_4);
body_swing_l_4->EnableCollision(false);
//layer 4
auto body_swing_r_5 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
0.1, 0.1, 1000, material); body_swing_r_5->SetFixed(true);
body_swing_r_5->SetPos(frame_gripper_1 * ChVector3d(0, 0, 0.1 / 2));
body_swing_r_5->SetRot(frame_gripper_1.GetRot()); sys.AddBody(body_swing_r_5);
body_swing_r_5->EnableCollision(false);
auto body_swing_l_5 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
0.1, 0.1, 1000, material); body_swing_l_5->SetFixed(true);
body_swing_l_5->SetPos(frame_gripper_2 * ChVector3d(0, 0, 0.1 / 2));
body_swing_l_5->SetRot(frame_gripper_2.GetRot()); sys.AddBody(body_swing_l_5);
body_swing_l_5->EnableCollision(false);
//gripper
/*auto body_gripper_1 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
0.1, 0.1, 100, material); body_gripper_1->SetFixed(true);
body_gripper_1->SetPos(frame_gripper_1.GetPos());
body_gripper_1->SetRot(frame_gripper_1.GetRot()); sys.AddBody(body_gripper_1);
body_gripper_1->EnableCollision(false);
auto body_gripper_2 = chrono_types::make_shared<ChBodyEasyBox>(0.1,
0.1, 0.1, 100, material); body_gripper_2->SetFixed(true);
body_gripper_2->SetPos(frame_gripper_2.GetPos());
body_gripper_2->SetRot(frame_gripper_2.GetRot()); sys.AddBody(body_gripper_2);
body_gripper_2->EnableCollision(false);*/
//right
auto set_f_r_1 =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto set_f_r_2 =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto set_f_r_3 =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto set_f_r_4 =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto motor_r_1 = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
auto motor_r_2 = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
auto motor_r_3 = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
auto motor_r_4 = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
motor_r_1->Initialize(body_swing_r_1, body_swing, frame_joint_r_1);
motor_r_1->SetSpeedFunction(set_f_r_1); sys.AddLink(motor_r_1);
//motor_r_1->SetAngleOffset(-initial_angle_shl_r);
motor_r_2->Initialize(body_swing_r_1, body_swing_r_2, frame_joint_r_2);
sys.AddLink(motor_r_2); motor_r_2->SetSpeedFunction(set_f_r_2);
//motor_r_2->SetAngleOffset(-initial_angle_plane_body_r);
motor_r_3->Initialize(body_swing_r_2, body_swing_r_3, frame_joint_r_3);
motor_r_3->SetSpeedFunction(set_f_r_3); sys.AddLink(motor_r_3);
//motor_r_1->SetAngleOffset(-initial_angle_shl_r);
motor_r_4->Initialize(body_swing_r_4, body_swing_r_5, frame_gripper_1);
sys.AddLink(motor_r_4); motor_r_4->SetSpeedFunction(set_f_r_4);
//left
auto set_f_l_1 =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto set_f_l_2 =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto set_f_l_3 =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto set_f_l_4 =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto motor_l_1 = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
auto motor_l_2 = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
auto motor_l_3 = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
auto motor_l_4 = chrono_types::make_shared<ChLinkMotorRotationSpeed>();
motor_l_1->Initialize(body_swing_l_1, body_swing, frame_joint_l_1);
motor_l_1->SetSpeedFunction(set_f_l_1); sys.AddLink(motor_l_1);
//motor_l_1->SetAngleOffset(-initial_angle_shl_r);
motor_l_2->Initialize(body_swing_l_1, body_swing_l_2, frame_joint_l_2);
sys.AddLink(motor_l_2); //motor_l_2->SetSpeedFunction(set_f_l_2);
//motor_l_2->SetAngleOffset(-initial_angle_plane_body_r);
motor_l_3->Initialize(body_swing_l_2, body_swing_l_3, frame_joint_l_3);
sys.AddLink(motor_l_3);//motor_l_3->SetSpeedFunction(set_f_l_3);
//motor_l_1->SetAngleOffset(-initial_angle_shl_r);
motor_l_4->Initialize(body_swing_l_4, body_swing_l_5, frame_gripper_2);
sys.AddLink(motor_l_4); //motor_l_4->SetSpeedFunction(set_f_l_4);
//motor_l_4->SetDisabled(true);
//motor_l_3->SetDisabled(true);
auto joint_helper_l_4 = chrono_types::make_shared
<ChLinkLockRevolute>();
auto joint_helper_l_3 = chrono_types::make_shared
<ChLinkLockRevolute>();
//joint_helper_l_4->Initialize(body_swing_l_4, body_swing_l_5,
frame_gripper_2); sys.AddLink(joint_helper_l_4);
//joint_helper_l_3->Initialize(body_swing_l_2, body_swing_l_3,
frame_joint_l_3); sys.AddLink(joint_helper_l_3);
// joint
/*auto joint_main_r_1 = chrono_types::make_shared
<ChLinkLockRevolute>();
auto joint_main_l_1 = chrono_types::make_shared <ChLinkLockRevolute>();
joint_main_r_1->Initialize(body_swing_r_1, body_swing,
frame_joint_r_1); sys.AddLink(joint_main_r_1);
joint_main_l_1->Initialize(body_swing_l_1, body_swing,
frame_joint_l_1); sys.AddLink(joint_main_l_1);
auto joint_main_r_2 = chrono_types::make_shared <ChLinkLockRevolute>();
auto joint_main_l_2 = chrono_types::make_shared <ChLinkLockRevolute>();
joint_main_r_2->Initialize(body_swing_r_2, body_swing_r_1,
frame_joint_r_2); sys.AddLink(joint_main_r_2);
joint_main_l_2->Initialize(body_swing_l_2, body_swing_l_1,
frame_joint_l_2); sys.AddLink(joint_main_l_2);*/
auto joint_1 = chrono_types::make_shared
<ChLinkUniversal>();//ChLinkUniversal
auto joint_2 = chrono_types::make_shared <
ChLinkUniversal>();//ChLinkLockSpherical
joint_1->Initialize(body_swing_r_3, body_swing_r_4, frame_joint_r_5);
sys.AddLink(joint_1);
joint_2->Initialize(body_swing_l_3, body_swing_l_4, frame_joint_l_5);
sys.AddLink(joint_2);
// solver part
auto solver = chrono_types::make_shared<ChSolverBB>();//ChSolverMINRES,
ChSolverPJacobi,ChSolverBB,ADMM
// Attach solver to system
sys.SetSolver(solver);
sys.SetTimestepperType(ChTimestepper::Type::EULER_IMPLICIT_LINEARIZED);//EULER_IMPLICIT_LINEARIZED
// Configure the integrator
auto stepper =
std::dynamic_pointer_cast<ChTimestepperHHT>(sys.GetTimestepper());
if (stepper) {
stepper->SetAlpha(-0.3); // numerical damping (-0.3..0
recommended)
stepper->SetMaxIters(100); // iterations per step
stepper->SetAbsTolerances(1e-12, 1e-12); // state & constraint
tolerance
}
double timestep = time_step;
ChRealtimeStepTimer realtime_timer;
auto vis = chrono_types::make_shared<ChVisualSystemIrrlicht>();
vis->AttachSystem(&sys);
vis->SetWindowSize(800, 600);
vis->SetWindowTitle("testing_body_system");
vis->Initialize();
vis->AddLogo();
vis->AddSkyBox();
vis->AddTypicalLights();
vis->AddCamera(ChVector3d(0, 4, 4));
vis->AddLightWithShadow(ChVector3d(20.0, 35.0, -25.0), ChVector3d(0, 0,
0), 55, 20, 55, 35, 512,
ChColor(0.6f, 0.8f, 1.0f));
vis->EnableShadows();
int mode = ChCollisionSystem::VIS_Shapes;
bool use_zbuffer = true;
//double start_angle = motor_r_4->GetAngleOffset();
double contions_pos;
while (vis->Run()) {
vis->BeginScene();
vis->Render();
//tools::drawCoordsys(vis.get(), frame_joint_l_1.GetCoordsys());
//tools::drawCoordsys(vis.get(), frame_joint_r_1.GetCoordsys());
//tools::drawCoordsys(vis.get(), frame_joint_l_2.GetCoordsys());
//tools::drawCoordsys(vis.get(), frame_joint_r_2.GetCoordsys());
//tools::drawCoordsys(vis.get(),
joint_r_4->GetFrame1Abs().GetCoordsys());
//tools::drawCoordsys(vis.get(),
joint_r_4->GetFrame2Abs().GetCoordsys());
//tools::drawCoordsys(vis.get(),
joint_r_u->GetFrame1Abs().GetCoordsys());
//tools::drawCoordsys(vis.get(),
joint_r_u->GetFrame2Abs().GetCoordsys());
double t = sys.GetChTime();
std::cout << t << "\n";
vis->EndScene();
//set_f_r_1->SetSetpoint(1, t);
//set_f_r_2->SetSetpoint(1, t);
//set_f_r_3->SetSetpoint(1, t);
sys.DoStepDynamics(timestep);
realtime_timer.Spin(timestep);
}
}
int main() {
test_system_rigid_body_2();
return 0;
}
