I am currently trying to simulate a climbing robot, for this I need to set
up the arms which are built from actuated joints (with motors) and passive
actuated joints (spring loaded). When using 4 TSDA-Links (spring coef is
5000) and a universal joint built by defining a generic joint( allowing two
rotational dofs), there is an unexpected movement (in the simulation there
are translational movements visible). I want to attach moveable grippers to
the end effector and the main body be movable as well. The TSDA needs to be
stiff. I am using Solver BB and for the Time stepper
EULER_IMPLICIT_LINEARIZED. How can avoid this jumping motion, is there an
solver time stepper setup for most accurate results.
Best regards from LUT
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/04aa6e91-a625-475d-aeca-106d288dd019n%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;
double sphereswept_r = 0.001;
bool draw_coll_shapes = true;
ChVisualSystem::Type vis_type = ChVisualSystem::Type::IRRLICHT;
ChContactMethod contact_method = ChContactMethod::NSC;
ChCollisionSystem::Type collision_type = ChCollisionSystem::Type::BULLET;
void get_body_add(std::shared_ptr<ChBodyAuxRef>& body, std::string name,
ChFramed frame, ChFramed frame_COM, std::string link_vis, std::string
link_collision, double mass, chrono::ChVector3d InertiaXX, chrono::ChVector3d
InertXY, std::shared_ptr<chrono::ChContactMaterialNSC> mat_1, bool colison) {
std::cout << "2" << " " << "\n";
std::shared_ptr<chrono::ChVisualShapeModelFile> body_shape;
chrono::ChMatrix33<> mr;
body = chrono_types::make_shared<ChBodyAuxRef>();
body->SetName(name); std::cout << "2" << " " << "\n";
body->SetPos(frame.GetPos());
body->SetRot(frame.GetRot());
body->SetMass(mass);
body->SetInertiaXX(InertiaXX);
body->SetInertiaXY(InertXY);
body->SetFrameCOMToRef(frame_COM);
std::cout << "2" << " " << "\n";
body_shape =
chrono_types::make_shared<chrono::ChVisualShapeModelFile>();
body_shape->SetFilename(link_vis);
body->AddVisualShape(body_shape, chrono::ChFramed(chrono::ChVector3d(0,
0, 0), chrono::ChQuaternion<>(1, 0, 0, 0)));
std::cout << "2" << " " << "\n";
// Collision Model
body->AddCollisionModel(chrono_types::make_shared<chrono::ChCollisionModel>());
// Collision material
// Collision shape
std::shared_ptr<chrono::ChCollisionShape> collshape_1;
// Triangle mesh collision shape
//auto body_1_1_collision_mesh =
chrono_types::make_shared<chrono::ChTriangleMeshConnected>();
//body_1_1_collision_mesh->LoadWavefrontMesh(link_vis, false, true);
auto body_1_1_collision_mesh =
ChTriangleMeshConnected::CreateFromWavefrontFile(link_collision,
false, true);
mr(0, 0) = 1; mr(1, 0) = 0; mr(2, 0) = 0;
mr(0, 1) = 0; mr(1, 1) = 1; mr(2, 1) = 0;
mr(0, 2) = 0; mr(1, 2) = 0; mr(2, 2) = 1;
body_1_1_collision_mesh->Transform(chrono::ChVector3d(0, 0, 0),
ChMatrix33<>(1));
collshape_1 =
chrono_types::make_shared<chrono::ChCollisionShapeTriangleMesh>(mat_1,
body_1_1_collision_mesh, false, false, sphereswept_r);
body->GetCollisionModel()->AddShape(collshape_1);
body->EnableCollision(colison);
// Visualization shape
}
// Drawer for collision shapes
class DebugDrawer : public ChCollisionSystem::VisualizationCallback {
public:
explicit DebugDrawer(ChVisualSystemIrrlicht* vis) : m_vis(vis) {}
~DebugDrawer() {}
virtual void DrawLine(const ChVector3d& from, const ChVector3d& to,
const ChColor& color) override {
m_vis->GetVideoDriver()->draw3DLine(irr::core::vector3dfCH(from),
irr::core::vector3dfCH(to),
irr::video::SColor(255, 55, 55, 255));
}
virtual double GetNormalScale() const override { return 1.0; }
void Draw(int flags, bool use_zbuffer = true) {
m_vis->GetVideoDriver()->setTransform(irr::video::ETS_WORLD,
irr::core::matrix4());
irr::video::SMaterial mattransp;
mattransp.ZBuffer = use_zbuffer;
mattransp.Lighting = false;
m_vis->GetVideoDriver()->setMaterial(mattransp);
m_vis->GetSystem(0).GetCollisionSystem()->Visualize(flags);
}
private:
ChVisualSystemIrrlicht* m_vis;
};
void gym() {
// Create a Chrono system
ChSystemNSC sys;
ChCollisionModel::SetDefaultSuggestedEnvelope(0.001);
ChCollisionModel::SetDefaultSuggestedMargin(0.001);
sys.SetCollisionSystemType(collision_type);
auto material = chrono_types::make_shared<ChContactMaterialNSC>();
auto floor = chrono_types::make_shared<ChBodyEasyBox>(8, 0.3, 8, 1000,
material);
floor->SetFixed(true);
floor->EnableCollision(true);
sys.Add(floor);
double l = 2;
//double l2 = l;
//_1
auto pole = chrono_types::make_shared<ChBodyEasyCylinder>(ChAxis::Y,
0.048 / 2, l, 1000, material);
pole->EnableCollision(true);
pole->SetPos(ChVector3d(0, l / 2, 0));
sys.Add(pole);
pole->SetFixed(true);
// defining Parameters:
double dis = 0.3;
double l00_z_offset = 0.3;
double l0_x = 0.1;
double l0_y = 0.2;
double l1_1 = 0.2;
double l1_2 = 0.1;
double l2 = 0.35;
double l3 = 0.35;
double ls = 0.1;
double l0_rear = 0.25;
double l1_rear = 0.1;
double l2_rear = 0.2;
double l3_rear = 0.3;
double l4_gripper_rear = ls;
// defining frames
auto initial_frame = pole->GetVisualModelFrame();
auto frame_block_ee_1 = initial_frame * ChFrame(ChVector3d(0, dis, 0));
auto frame_block_ee_2 = initial_frame * ChFrame(ChVector3d(0, -dis, 0),
QuatFromAngleY(CH_PI));
auto frame_base_body=initial_frame* ChFrame(ChVector3d(0, 0,
l00_z_offset));
double initial_angle_shl_r = 0;//CH_PI*0.3;// at CH_PI error occurr
double initial_angle_plane_body_r = CH_PI_2*0.5;//0;//
double initial_angle_plane_angle_r = 0;//0;//
//
double initial_angle_shl_l = 0;// CH_PI * 0.3;//
double initial_angle_plane_body_l = 0;//CH_PI * -0.1;//
double initial_angle_plane_angle_l = 0;//CH_PI * 0.2;// right_arm
// frames
// right
auto frame_shr_pos_r= frame_base_body * ChFrame(ChVector3d(l0_x, l0_y,
0), QuatFromAngleY(CH_PI_4));
auto frame_shr_r_helper = frame_shr_pos_r * ChFrame(VNULL,
QuatFromAngleZ(initial_angle_shl_r));
auto frame_shlr_r = frame_shr_r_helper * ChFrame(VNULL,
QuatFromAngleY(-CH_PI_2) * QuatFromAngleZ(-CH_PI_2));
auto frame_joint_plane_body_r = ChFrame(frame_shlr_r * ChVector3d(0,
l1_1, l1_2), frame_shlr_r.GetRot() * QuatFromAngleY(-CH_PI) *
QuatFromAngleZ(initial_angle_plane_body_r));
auto frame_joint_plane_angle_r = ChFrame(frame_joint_plane_body_r *
ChVector3d(0, l2, 0), frame_joint_plane_body_r.GetRot() *
QuatFromAngleZ(initial_angle_plane_angle_r));
auto frame_joint_uni_r = ChFrame(frame_joint_plane_angle_r *
ChVector3d(0, l3 + 0.025, 0), frame_joint_plane_angle_r.GetRot() *
Q_ROTATE_Z_TO_Y);
auto frame_joint_after_u_r = frame_joint_uni_r * ChFrame(ChVector3d(0,
0, 0.025));
// left
// right
// // bodies
auto body = chrono_types::make_shared<ChBodyEasyBox>(0.1, 0.4, 0.05,
1000, material);
body->SetFixed(true);
body->SetPos(frame_base_body.GetPos());
body->EnableCollision(true);
sys.Add(body);
bool all_fixed_r = true;
auto body_conector_r = chrono_types::make_shared<ChBodyEasyBox>(0.05,
l1_1, 0.05, 1000, material); body_conector_r->SetFixed(all_fixed_r);
body_conector_r->SetPos(frame_shlr_r * ChVector3d(0, l1_1 / 2, 0));
body_conector_r->SetRot(frame_shlr_r.GetRot()); sys.AddBody(body_conector_r);
body_conector_r->EnableCollision(false);
auto body_link_arm_2_r = chrono_types::make_shared<ChBodyEasyBox>(0.05,
l2, 0.05, 1000, material); body_link_arm_2_r->SetFixed(all_fixed_r);
body_link_arm_2_r->SetPos(frame_joint_plane_body_r * ChVector3d(0, l2 / 2, 0));
body_link_arm_2_r->SetRot(frame_joint_plane_body_r.GetRot()); sys.AddBody(
body_link_arm_2_r); body_link_arm_2_r->EnableCollision(false);
auto body_link_arm_3_r = chrono_types::make_shared<ChBodyEasyBox>(0.05,
l3, 0.05, 1000, material); body_link_arm_3_r->SetFixed(all_fixed_r);
body_link_arm_3_r->SetPos(frame_joint_plane_angle_r * ChVector3d(0, l2 / 2,
0)); body_link_arm_3_r->SetRot(frame_joint_plane_angle_r.GetRot());
sys.AddBody(body_link_arm_3_r); body_link_arm_3_r->EnableCollision(false);
auto body_arm_r_h_u=
chrono_types::make_shared<ChBodyEasyCylinder>(ChAxis::Z,0.01, 0.002, 1000,
material); body_arm_r_h_u->SetFixed(false);
body_arm_r_h_u->SetPos(frame_joint_after_u_r * ChVector3d(0, 0, -0.001));
body_arm_r_h_u->SetRot(frame_joint_after_u_r.GetRot());
sys.AddBody(body_arm_r_h_u); body_arm_r_h_u->EnableCollision(false);
//joint
auto joint_r_1 = chrono_types::make_shared<ChLinkLockRevolute>();
auto joint_r_2 = chrono_types::make_shared<ChLinkLockRevolute>();
auto joint_r_3 = chrono_types::make_shared<ChLinkLockRevolute>();
auto joint_r_u =
chrono_types::make_shared<ChLinkMateGeneric>(true,true,true,false,false,true);//ChLinkUniversal
auto joint_r_4 = chrono_types::make_shared<ChLinkLockRevolute>();
if (!all_fixed_r){
joint_r_1->Initialize(body, body_conector_r, frame_shr_r_helper);
sys.AddLink(joint_r_1);
joint_r_2->Initialize(body_conector_r,body_link_arm_2_r,
frame_joint_plane_body_r); sys.AddLink(joint_r_2);
joint_r_3->Initialize(body_link_arm_2_r, body_link_arm_3_r,
frame_joint_plane_angle_r); sys.AddLink(joint_r_3);}
joint_r_u->Initialize(body_link_arm_3_r, body_arm_r_h_u,
frame_joint_uni_r); sys.AddLink(joint_r_u); // muss evtl geändert werden
//motors
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<ChLinkMotorRotationAngle>();
auto motor_r_2 = chrono_types::make_shared<ChLinkMotorRotationAngle>();
auto motor_r_3 = chrono_types::make_shared<ChLinkMotorRotationAngle>();
auto motor_r_4 = chrono_types::make_shared<ChLinkMotorRotationAngle>();
if (!all_fixed_r) {
motor_r_1->Initialize(body, body_conector_r,
frame_shr_r_helper); motor_r_1->SetAngleFunction(set_f_r_1);
sys.AddLink(motor_r_1); //motor_r_1->SetAngleOffset(-initial_angle_shl_r);
motor_r_2->Initialize(body_conector_r, body_link_arm_2_r,
frame_joint_plane_body_r); sys.AddLink(motor_r_2);
motor_r_2->SetAngleFunction(set_f_r_2);
//motor_r_2->SetAngleOffset(-initial_angle_plane_body_r);
motor_r_3->Initialize(body_link_arm_2_r, body_link_arm_3_r,
frame_joint_plane_angle_r); sys.AddLink(motor_r_3);
motor_r_3->SetAngleFunction(set_f_r_3);
motor_r_3->SetAngleOffset(-initial_angle_plane_angle_r);
}
// springs
double rest_length = 0.08;
double spring_coef = 500;// 50;
double damping_coef = 10;
auto spring_1_r = chrono_types::make_shared<ChLinkTSDA>();
spring_1_r->Initialize(body_link_arm_3_r, body_arm_r_h_u, true,
ChVector3d(0, l3 / 2 - 0.05, -0.02), ChVector3d(0, 0.05, 0));
spring_1_r->SetRestLength(rest_length);
spring_1_r->SetSpringCoefficient(spring_coef);
spring_1_r->SetDampingCoefficient(damping_coef);
// --- 2. Feder ---
auto spring2_r = chrono_types::make_shared<ChLinkTSDA>();
spring2_r->Initialize(body_link_arm_3_r, body_arm_r_h_u, true,
ChVector3d(0.02, l3 / 2 - 0.05, 0), ChVector3d(0.05, 0, 0));
spring2_r->SetSpringCoefficient(spring_coef);
spring2_r->SetDampingCoefficient(damping_coef);
spring2_r->SetRestLength(rest_length);
// --- 3. Feder ---
auto spring3_r = chrono_types::make_shared<ChLinkTSDA>();
spring3_r->Initialize(body_link_arm_3_r, body_arm_r_h_u, true,
ChVector3d(-0.02, l3 / 2 - 0.05, 0), ChVector3d(-0.05, 0, 0));
spring3_r->SetSpringCoefficient(spring_coef);
spring3_r->SetDampingCoefficient(damping_coef);
spring3_r->SetRestLength(rest_length);
// --- 4. Feder ---
auto spring4_r = chrono_types::make_shared<ChLinkTSDA>();
spring4_r->Initialize(body_link_arm_3_r, body_arm_r_h_u, true,
ChVector3d(0, l3 / 2 - 0.05, 0.02), ChVector3d(0, -0.05, 0));
spring4_r->SetSpringCoefficient(spring_coef);
spring4_r->SetDampingCoefficient(damping_coef);
spring4_r->SetRestLength(rest_length);
sys.AddLink(spring_1_r);
sys.AddLink(spring2_r);
sys.AddLink(spring3_r);
sys.AddLink(spring4_r);
// Attach a visualization asset.
spring_1_r->AddVisualShape(chrono_types::make_shared<ChVisualShapeSpring>(0.01,
40, 8));
spring2_r->AddVisualShape(chrono_types::make_shared<ChVisualShapeSpring>(0.01,
40, 8));
spring3_r->AddVisualShape(chrono_types::make_shared<ChVisualShapeSpring>(0.01,
40, 8));
spring4_r->AddVisualShape(chrono_types::make_shared<ChVisualShapeSpring>(0.01,
40, 8));
// grippers
auto material_friction =
chrono_types::make_shared<ChContactMaterialNSC>();
material_friction->SetFriction(0.5);
material_friction->SetRollingFriction(0.5);
material_friction->SetSlidingFriction(0.5);
std::string shapes_dir =
"C:\\workspace\\climbing_101\\gripper_export_shapes\\";
std::shared_ptr<ChBodyAuxRef> finger_1_r;
std::shared_ptr<ChBodyAuxRef> finger_2_r;
std::shared_ptr<ChBodyAuxRef> base_r;
auto frame_base_com_to_ref =
chrono::ChFramed(chrono::ChVector3d(1.88969192858969e-18, 0.0466317534677547,
4.71445555287807e-19), chrono::ChQuaternion<>(1, 0, 0, 0));
auto frame_finger_1_com_to_ref =
chrono::ChFramed(chrono::ChVector3d(0.000953801080669733, -0.021322686682018,
5.3115175771791e-19), chrono::ChQuaternion<>(1, 0, 0, 0));
auto frame_finger_2_com_to_ref =
chrono::ChFramed(chrono::ChVector3d(0.000953801080669733, -0.021322686682018,
5.3115175771791e-19), chrono::ChQuaternion<>(1, 0, 0, 0));
auto frame_gripper_r = frame_joint_after_u_r * ChFrame(ChVector3d(0, 0,
0), QuatFromAngleX(CH_PI_2) * QuatFromAngleY(CH_PI_2));
auto frame_finger_1_r = frame_gripper_r *
ChFrame(ChVector3d(0.0366618867860707, -0.003116191915022,
0.0591554353444595)).GetInverse() * ChFrame(ChVector3d(-0.00148250962951285,
0.137595735538563, 0.0591554353444594));
auto frame_finger_2_r = frame_gripper_r *
ChFrame(ChVector3d(0.0366618867860707, -0.003116191915022,
0.0591554353444595)).GetInverse() *
ChFrame(chrono::ChVector3d(0.0743899226468857, 0.137595735538563,
0.0591554353444595), chrono::ChQuaternion<>(7.85000728502012e-17,
-1.17368952349279e-16, 1, 2.77555756156289e-17));
get_body_add(base_r, std::string("base"), frame_gripper_r,
frame_base_com_to_ref, shapes_dir + "body_3_1.obj", shapes_dir +
"body_3_1_collision.obj", 0.406216270935814,
chrono::ChVector3d(0.000352637121668769, 0.000451309361507021,
0.000370829111668769), chrono::ChVector3d(-1.06257411112441e-20,
-1.28013920144965e-20, -8.61559598459534e-21), material_friction, true);
sys.AddBody(base_r);
/*get_body_add(finger_1_r, std::string("finger_1"), frame_finger_1_r,
frame_finger_1_com_to_ref, shapes_dir + "body_1_1_collision_mini.obj",
shapes_dir + "body_1_1_collision_mini.obj", 0.0373760124348794,
chrono::ChVector3d(4.54111393779954e-05, 5.14947357705548e-06,
4.11685501697624e-05), chrono::ChVector3d(-3.20274862402208e-07,
1.8446335448084e-22, -1.03000641621034e-21), material_friction, false);//
Inertia and COM and position are done by solidworks export
get_body_add(finger_2_r, std::string("finger_2"), frame_finger_2_r,
frame_finger_2_com_to_ref, shapes_dir + "body_1_1_collision_mini.obj",
shapes_dir + "body_1_1_collision_mini.obj", 0.0373760124348794,
chrono::ChVector3d(4.54111393779954e-05, 5.14947357705548e-06,
4.11685501697624e-05), ChVector3d(3.20274862402217e-07, -4.816237693587e-22,
-3.59090401274159e-21), material_friction, false);
sys.AddBody(finger_1_r); sys.AddBody(finger_2_r);
finger_1_r->EnableCollision(true); finger_2_r->EnableCollision(true);
double limit_neg = -0.01;
double limit_pos = 0.03;
//right
auto frame_link_1_r = frame_finger_1_r * ChFrame(ChVector3d(0, -0.065,
0), QuatFromAngleY(CH_PI_2));
auto frame_link_2_r = frame_finger_2_r * ChFrame(ChVector3d(0, -0.065,
0), QuatFromAngleY(CH_PI_2));
auto prismatic_finger_1_r =
chrono_types::make_shared<ChLinkLockPrismatic>();
prismatic_finger_1_r->Initialize(finger_1_r, base_r, frame_link_1_r);
prismatic_finger_1_r->LimitZ().SetActive(true);
prismatic_finger_1_r->LimitZ().SetMin(limit_neg);
prismatic_finger_1_r->LimitZ().SetMax(limit_pos);
sys.AddLink(prismatic_finger_1_r);
// finger_2 base
auto prismatic_finger_2_r =
chrono_types::make_shared<ChLinkLockPrismatic>();
prismatic_finger_2_r->Initialize(finger_2_r, base_r, frame_link_2_r);
prismatic_finger_2_r->LimitZ().SetActive(true);
prismatic_finger_2_r->LimitZ().SetMin(limit_pos);
prismatic_finger_2_r->LimitZ().SetMax(limit_pos);
sys.AddLink(prismatic_finger_2_r);
auto motfun_finger_1_r =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto motfun_finger_2_r =
chrono_types::make_shared<chrono::ChFunctionSetpoint>();
auto motor_finger_1_r =
chrono_types::make_shared<chrono::ChLinkMotorLinearForce>();
motor_finger_1_r->Initialize(finger_1_r, base_r, frame_link_1_r);
sys.AddLink(motor_finger_1_r);
motor_finger_1_r->SetMotorFunction(motfun_finger_1_r);
auto motor_finger_2_r =
chrono_types::make_shared<chrono::ChLinkMotorLinearForce>();
motor_finger_2_r->Initialize(finger_2_r, base_r, frame_link_2_r);
sys.AddLink(motor_finger_2_r);
motor_finger_2_r->SetMotorFunction(motfun_finger_2_r);
prismatic_finger_1_r->Lock(true);
prismatic_finger_2_r->Lock(true);*/
joint_r_4->Initialize(body_arm_r_h_u, base_r,
body_arm_r_h_u->GetVisualModelFrame()); sys.AddLink(joint_r_4);
motor_r_4->Initialize(body_arm_r_h_u, base_r,
body_arm_r_h_u->GetVisualModelFrame()); motor_r_4->SetAngleFunction(set_f_r_4);
sys.AddLink(motor_r_4);
//left
auto solver =
chrono_types::make_shared<ChSolverBB>();//ChSolverMINRES,
ChSolverPJacobi,ChSolverBB,ADMM
// Set maximum iterations and tolerance
//solver->SetMaxIterations(400); // number of solver
iterations
//solver->SetTolerance(1e-10); // constraint
tolerance
//solver->EnableDiagonalPreconditioner(true); // often helps
// 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.2); // numerical damping
(-0.3..0 recommended)
stepper->SetMaxIters(100); // iterations per step
stepper->SetAbsTolerances(1e-12, 1e-12); // state &
constraint tolerance
}
double timestep = 0.001;
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();
auto drawer = chrono_types::make_shared<DebugDrawer>(vis.get());
sys.GetCollisionSystem()->RegisterVisualizationCallback(drawer);
int mode = ChCollisionSystem::VIS_Shapes;
bool use_zbuffer = true;
double start_angle = motor_r_4->GetAngleOffset();
while (vis->Run()) {
vis->BeginScene();
vis->Render();
if (draw_coll_shapes)
drawer->Draw(mode, use_zbuffer);
tools::drawCoordsys(vis.get(),
frame_shr_r_helper.GetCoordsys());
std::cout << spring_1_r->GetLength()<<"\n";
vis->EndScene();
double t = sys.GetChTime();
std::cout << t << "\n";
if (t > 0.2) {
}
else {
double val = start_angle + t * 2;
set_f_r_4->SetSetpoint(val, t);
}
sys.DoStepDynamics(timestep);
realtime_timer.Spin(timestep);
}
}
int main() {
gym();
return 0;
}
