From 4877fc4c2b7b5413ae517d6485c082a92e4d1d64 Mon Sep 17 00:00:00 2001 From: dtotsila Date: Tue, 6 Aug 2024 17:54:26 +0200 Subject: [PATCH] add comments to pendulum.py example #212 --- src/docs/include/macros.py | 2 +- src/examples/pendulum.cpp | 7 +++++-- src/examples/python/pendulum.py | 5 +++++ 3 files changed, 11 insertions(+), 3 deletions(-) diff --git a/src/docs/include/macros.py b/src/docs/include/macros.py index e32f9f62..c567d51a 100644 --- a/src/docs/include/macros.py +++ b/src/docs/include/macros.py @@ -1,5 +1,5 @@ def define_env(env): - variables = {'SET_ACTUATOR': '\t```cpp\n\t// Set all DoFs to same actuator\n\trobot->set_actuator_types("servo"); // actuator types can be "servo", "torque", "velocity", "passive", "locked", "mimic"\n\t// You can also set actuator types separately\n\trobot->set_actuator_type("torque", "iiwa_joint_5");\n\t```', 'POSITIONS_ENFORCED': '\t```cpp\n\t// Εnforce joint position and velocity limits\n\trobot->set_position_enforced(true);\n\t```', 'MODIFY_LIMITS': '\t```cpp\n\t// Modify Position Limits\n\tEigen::VectorXd pos_upper_lims(7);\n\tpos_upper_lims << 2.096, 2.096, 2.096, 2.096, 2.096, 2.096, 2.096;\n\trobot->set_position_upper_limits(pos_upper_lims, dof_names);\n\trobot->set_position_lower_limits(-pos_upper_lims, dof_names);\n\t\n\t// Modify Velocity Limits\n\t\n\tEigen::VectorXd vel_upper_lims(7);\n\tvel_upper_lims << 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5;\n\trobot->set_velocity_upper_limits(vel_upper_lims, dof_names);\n\trobot->set_velocity_lower_limits(-vel_upper_lims, dof_names);\n\t\n\t// Modify Force Limits\n\t\n\tEigen::VectorXd force_upper_lims(7);\n\tforce_upper_lims << 150, 150, 150, 150, 150, 150, 150;\n\trobot->set_force_upper_limits(force_upper_lims, dof_names);\n\trobot->set_force_lower_limits(-force_upper_lims, dof_names);\n\t\n\t// Modify Acceleration Limits\n\tEigen::VectorXd acc_upper_lims(7);\n\tacc_upper_lims << 1500, 1500, 1500, 1500, 1500, 1500, 1500;\n\trobot->set_acceleration_upper_limits(acc_upper_lims, dof_names);\n\trobot->set_acceleration_lower_limits(-acc_upper_lims, dof_names);\n\t```', 'MODIFY_COEFFS': '\t```cpp\n\t// Modify Damping Coefficients\n\tstd::vector damps = {0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4};\n\trobot->set_damping_coeffs(damps, dof_names);\n\t\n\t// Modify Coulomb Frictions\n\tstd::vector cfrictions = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001};\n\trobot->set_coulomb_coeffs(cfrictions, dof_names);\n\t\n\t// Modify Spring Stiffness\n\tstd::vector stiffnesses = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001};\n\trobot->set_spring_stiffnesses(stiffnesses, dof_names);\n\t```', 'SET_COLLISION_DETECTOR': '\t```cpp\n\tsimu.set_collision_detector("fcl"); // collision_detector can be "dart", "fcl", "ode" or "bullet" (case does not matter)\n\t```', 'SELF_COLLISIONS': '\t```cpp\n\tif (!robot->self_colliding()) {\n\t std::cout << "Self collision is not enabled" << std::endl;\n\t // set self collisions to true and adjacent collisions to false\n\t robot->set_self_collision(true, false);\n\t}\n\t```', 'SIMPLE_ARM': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'CAMERAS_PARALLEL': '\t```cpp\n\t// Load robot from URDF\n\tauto global_robot = std::make_shared();\n\t\n\tstd::vector workers;\n\t\n\t// Set maximum number of parallel GL contexts (this is GPU-dependent)\n\trobot_dart::gui::magnum::GlobalData::instance()->set_max_contexts(4);\n\t\n\t// We want 15 parallel simulations with different GL context each\n\tsize_t N_workers = 15;\n\tstd::mutex mutex;\n\tsize_t id = 0;\n\t// Launch the workers\n\tfor (size_t i = 0; i < N_workers; i++) {\n\t workers.push_back(std::thread([&] {\n\t mutex.lock();\n\t size_t index = id++;\n\t mutex.unlock();\n\t\n\t // Get the GL context -- this is a blocking call\n\t // will wait until one GL context is available\n\t // get_gl_context(gl_context); // this call will not sleep between failed queries\n\t get_gl_context_with_sleep(gl_context, 20); // this call will sleep 20ms between each failed query\n\t\n\t // Do the simulation\n\t auto robot = global_robot->clone();\n\t\n\t robot_dart::RobotDARTSimu simu(0.001);\n\t\n\t Eigen::VectorXd ctrl = robot_dart::make_vector({0., M_PI / 3., 0., -M_PI / 4., 0., 0., 0.});\n\t\n\t auto controller = std::make_shared(ctrl);\n\t robot->add_controller(controller);\n\t controller->set_pd(300., 50.);\n\t\n\t // Magnum graphics\n\t robot_dart::gui::magnum::GraphicsConfiguration configuration = robot_dart::gui::magnum::WindowlessGraphics::default_configuration();\n\t\n\t configuration.width = 1024;\n\t configuration.height = 768;\n\t auto graphics = std::make_shared(configuration);\n\t simu.set_graphics(graphics);\n\t // Position the camera differently for each thread to visualize the difference\n\t graphics->look_at({0.4 * index, 3.5 - index * 0.1, 2.}, {0., 0., 0.25});\n\t // record images from main camera/graphics\n\t // graphics->set_recording(true); // WindowlessGLApplication records images by default\n\t\n\t simu.add_robot(robot);\n\t simu.run(6);\n\t\n\t // Save the image for verification\n\t robot_dart::gui::save_png_image("camera_" + std::to_string(index) + ".png",\n\t graphics->image());\n\t\n\t // Release the GL context for another thread to use\n\t release_gl_context(gl_context);\n\t }));\n\t}\n\t\n\t// Wait for all the workers\n\tfor (size_t i = 0; i < workers.size(); i++) {\n\t workers[i].join();\n\t}\n\t```', 'INIT_SIMU': '\t```cpp\n\t// choose time step of 0.001 seconds\n\trobot_dart::RobotDARTSimu simu(0.001);\n\t```', 'MODIFY_SIMU_DT': '\t```cpp\n\t// set timestep to 0.005 and update control frequency(bool)\n\tsimu.set_timestep(0.005, true);\n\t```', 'SIMU_GRAVITY': '\t```cpp\n\t// Set gravitational force of mars\n\tEigen::Vector3d mars_gravity = {0., 0., -3.721};\n\tsimu.set_gravity(mars_gravity);\n\t```', 'GRAPHICS_PARAMS': '\t```cpp\n\trobot_dart::gui::magnum::GraphicsConfiguration configuration;\n\t// We can change the width/height of the window (or camera image dimensions)\n\tconfiguration.width = 1280;\n\tconfiguration.height = 960;\n\tconfiguration.title = "Graphics Tutorial"; // We can set a title for our window\n\t\n\t// We can change the configuration for shadows\n\tconfiguration.shadowed = true;\n\tconfiguration.transparent_shadows = true;\n\tconfiguration.shadow_map_size = 1024;\n\t\n\t// We can also alter some specifications for the lighting\n\tconfiguration.max_lights = 3; // maximum number of lights for our scene [default=3]\n\tconfiguration.specular_strength = 0.25; // strength of the specular component\n\t\n\t// Some extra configuration for the main camera\n\tconfiguration.draw_main_camera = true;\n\tconfiguration.draw_debug = true;\n\tconfiguration.draw_text = true;\n\t\n\t// We can also change the background color [default=black]\n\tconfiguration.bg_color = Eigen::Vector4d{1.0, 1.0, 1.0, 1.0};\n\t\n\t// Create the graphics object with the configuration as parameter\n\tauto graphics = std::make_shared(configuration);\n\t```', 'SHADOWS_GRAPHICS': '\t```cpp\n\t// Disable shadows\n\tgraphics->enable_shadows(false, false);\n\tsimu.run(1.);\n\t// Enable shadows only for non-transparent objects\n\tgraphics->enable_shadows(true, false);\n\tsimu.run(1.);\n\t// Enable shadows for transparent objects as well\n\tgraphics->enable_shadows(true, true);\n\tsimu.run(1.);\n\t```', 'CLR_LIGHT': '\t```cpp\n\t// Clear Lights\n\tgraphics->clear_lights();\n\t```', 'LIGHT_MATERIAL': '\t```cpp\n\t// Create Light material\n\tMagnum::Float shininess = 1000.f;\n\tMagnum::Color4 white = {1.f, 1.f, 1.f, 1.f};\n\t\n\t// ambient, diffuse, specular\n\tauto custom_material = robot_dart::gui::magnum::gs::Material(white, white, white, shininess);\n\t```', 'POINT_LIGHT': '\t```cpp\n\t// Create point light\n\tMagnum::Vector3 position = {0.f, 0.f, 2.f};\n\tMagnum::Float intensity = 1.f;\n\tMagnum::Vector3 attenuation_terms = {1.f, 0.f, 0.f};\n\tauto point_light = robot_dart::gui::magnum::gs::create_point_light(position, custom_material, intensity, attenuation_terms);\n\tgraphics->add_light(point_light);\n\t```', 'DIRECTIONAL_LIGHT': '\t```cpp\n\t// Create directional light\n\tMagnum::Vector3 direction = {-1.f, -1.f, -1.f};\n\tauto directional_light = robot_dart::gui::magnum::gs::create_directional_light(direction, custom_material);\n\tgraphics->add_light(directional_light);\n\t```', 'SPOT_LIGHT': '\t```cpp\n\t// Create spot light\n\tMagnum::Vector3 position = {0.f, 0.f, 1.f};\n\tMagnum::Vector3 direction = {-1.f, -1.f, -1.f};\n\tMagnum::Float intensity = 1.f;\n\tMagnum::Vector3 attenuation_terms = {1.f, 0.f, 0.f};\n\tMagnum::Float spot_exponent = M_PI;\n\tMagnum::Float spot_cut_off = M_PI / 8;\n\tauto spot_light = robot_dart::gui::magnum::gs::create_spot_light(position, custom_material, direction, spot_exponent, spot_cut_off, intensity, attenuation_terms);\n\tgraphics->add_light(spot_light);\n\t```', 'LOAD_IICUB': '\t```cpp\n\tauto robot = std::make_shared();\n\t// Set actuator types to VELOCITY motors so that they stay in position without any controller\n\trobot->set_actuator_types("velocity");\n\trobot_dart::RobotDARTSimu simu(0.001);\n\tsimu.set_collision_detector("fcl");\n\t```', 'ICUB_PRINT_IMU': '\t```cpp\n\tstd::cout << "Angular Position: " << robot->imu().angular_position_vec().transpose().format(fmt) << std::endl;\n\tstd::cout << "Angular Velocity: " << robot->imu().angular_velocity().transpose().format(fmt) << std::endl;\n\tstd::cout << "Linear Acceleration: " << robot->imu().linear_acceleration().transpose().format(fmt) << std::endl;\n\tstd::cout << "=================================" << std::endl;\n\t```', 'ICUB_PRINT_FT': '\t```cpp\n\tstd::cout << "FT ( force): " << robot->ft_foot_left().force().transpose().format(fmt) << std::endl;\n\tstd::cout << "FT (torque): " << robot->ft_foot_left().torque().transpose().format(fmt) << std::endl;\n\tstd::cout << "=================================" << std::endl;\n\t```', 'CAMERA_SENSOR_RGBD_RECORD_DEPTH': '\t```cpp\n\tcamera->camera().record(true, true); // cameras are recording color images by default, enable depth images as well for this example\n\t```', 'TORQUE_SENSOR': '\t```cpp\n\t// Add torque sensors to the robot\n\tint ct = 0;\n\tstd::vector> tq_sensors(robot->num_dofs());\n\tfor (const auto& joint : robot->dof_names())\n\t tq_sensors[ct++] = simu.add_sensor(robot, joint, 1000);\n\t```', 'FORCE_TORQUE_SENSOR': '\t```cpp\n\t// Add force-torque sensors to the robot\n\tct = 0;\n\tstd::vector> f_tq_sensors(robot->num_dofs());\n\tfor (const auto& joint : robot->dof_names())\n\t f_tq_sensors[ct++] = simu.add_sensor(robot, joint, 1000, "parent_to_child");\n\t```', 'IMU_SENSOR': '\t```cpp\n\t// Add IMU sensors to the robot\n\tct = 0;\n\tstd::vector> imu_sensors(robot->num_bodies());\n\tfor (const auto& body_node : robot->body_names()) {\n\t // _imu(std::make_shared(sensor::IMUConfig(body_node("head"), frequency))),\n\t imu_sensors[ct++] = simu.add_sensor(robot_dart::sensor::IMUConfig(robot->body_node(body_node), 1000));\n\t}\n\t```', 'TORQUE_MEASUREMENT': '\t```cpp\n\t// vector that contains the torque measurement for every joint (scalar)\n\tEigen::VectorXd torques_measure(robot->num_dofs());\n\tfor (const auto& tq_sens : tq_sensors)\n\t torques_measure.block<1, 1>(ct++, 0) = tq_sens->torques();\n\t```', 'FORCE_TORQUE_MEASUREMENT': '\t```cpp\n\t// matrix that contains the torque measurement for every joint (3d vector)\n\tEigen::MatrixXd ft_torques_measure(robot->num_dofs(), 3);\n\t// matrix that contains the force measurement for every joint (3d vector)\n\tEigen::MatrixXd ft_forces_measure(robot->num_dofs(), 3);\n\t// matrix that contains the wrench measurement for every joint (6d vector)[torque, force]\n\tEigen::MatrixXd ft_wrench_measure(robot->num_dofs(), 6);\n\tct = 0;\n\tfor (const auto& f_tq_sens : f_tq_sensors) {\n\t ft_torques_measure.block<1, 3>(ct, 0) = f_tq_sens->torque();\n\t ft_forces_measure.block<1, 3>(ct, 0) = f_tq_sens->force();\n\t ft_wrench_measure.block<1, 6>(ct, 0) = f_tq_sens->wrench();\n\t ct++;\n\t}\n\t```', 'IMU_MEASUREMENT': '\t```cpp\n\tEigen::MatrixXd imu_angular_positions_measure(robot->num_bodies(), 3);\n\tEigen::MatrixXd imu_angular_velocities_measure(robot->num_bodies(), 3);\n\tEigen::MatrixXd imu_linear_acceleration_measure(robot->num_bodies(), 3);\n\tct = 0;\n\tfor (const auto& imu_sens : imu_sensors) {\n\t imu_angular_positions_measure.block<1, 3>(ct, 0) = imu_sens->angular_position_vec();\n\t imu_angular_velocities_measure.block<1, 3>(ct, 0) = imu_sens->angular_velocity();\n\t imu_linear_acceleration_measure.block<1, 3>(ct, 0) = imu_sens->linear_acceleration();\n\t ct++;\n\t}\n\t```', 'RGB_SENSOR': '\t```cpp\n\t// a nested std::vector (w*h*3) of the last image taken can be retrieved\n\tauto rgb_image = camera->image();\n\t```', 'RGB_SENSOR_MEASURE': '\t```cpp\n\t// we can also save them to png\n\trobot_dart::gui::save_png_image("camera-small.png", rgb_image);\n\t// convert an rgb image to grayscale (useful in some cases)\n\tauto gray_image = robot_dart::gui::convert_rgb_to_grayscale(rgb_image);\n\trobot_dart::gui::save_png_image("camera-gray.png", gray_image);\n\t```', 'RGB_D_SENSOR': '\t```cpp\n\t// get the depth image from a camera\n\t// with a version for visualization or bigger differences in the output\n\tauto rgb_d_image = camera->depth_image();\n\t// and the raw values that can be used along with the camera parameters to transform the image to point-cloud\n\tauto rgb_d_image_raw = camera->raw_depth_image();\n\t```', 'RGB_D_SENSOR_MEASURE': '\t```cpp\n\trobot_dart::gui::save_png_image("camera-depth.png", rgb_d_image);\n\trobot_dart::gui::save_png_image("camera-depth-raw.png", rgb_d_image_raw);\n\t```', 'FRANKA': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'PD_CONTROL': '\t```cpp\n\t// add a PD-controller to the arm\n\t// set desired positions\n\tEigen::VectorXd ctrl = robot_dart::make_vector({0., M_PI / 4., 0., -M_PI / 4., 0., M_PI / 2., 0., 0.});\n\t\n\t// add the controller to the robot\n\tauto controller = std::make_shared(ctrl);\n\trobot->add_controller(controller);\n\tcontroller->set_pd(300., 50.);\n\t```', 'ROBOT_CONTROL': '\t```cpp\n\tclass MyController : public robot_dart::control::RobotControl {\n\tpublic:\n\t MyController() : robot_dart::control::RobotControl() {}\n\t\n\t MyController(const Eigen::VectorXd& ctrl, bool full_control) : robot_dart::control::RobotControl(ctrl, full_control) {}\n\t MyController(const Eigen::VectorXd& ctrl, const std::vector& dof_names) : robot_dart::control::RobotControl(ctrl, dof_names) {}\n\t\n\t void configure() override\n\t {\n\t _active = true;\n\t }\n\t\n\t Eigen::VectorXd calculate(double) override\n\t {\n\t auto robot = _robot.lock();\n\t Eigen::VectorXd cmd = 100. * (_ctrl - robot->positions(_controllable_dofs));\n\t return cmd;\n\t }\n\t std::shared_ptr clone() const override\n\t {\n\t return std::make_shared(*this);\n\t }\n\t};\n\t```', 'TALOS': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'RECORD_VIDEO_ROBOT_GRAPHICS_PARAMS': '\t```cpp\n\tgraphics->record_video("talos_dancing.mp4");\n\t```', 'SIMPLE_CONTROL': '\t```cpp\n\tauto controller1 = std::make_shared(ctrl);\n\trobot->add_controller(controller1);\n\t```', 'A1': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'A1_PRINT_IMU': '\t```cpp\n\tstd::cout << "Angular Position: " << robot->imu().angular_position_vec().transpose().format(fmt) << std::endl;\n\tstd::cout << "Angular Velocity: " << robot->imu().angular_velocity().transpose().format(fmt) << std::endl;\n\tstd::cout << "Linear Acceleration: " << robot->imu().linear_acceleration().transpose().format(fmt) << std::endl;\n\tstd::cout << "=================================" << std::endl;\n\t```', 'HEXAPOD': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'HELLO_WORLD_INCLUDE': '\t```cpp\n\t#include \n\t\n\t#ifdef GRAPHIC\n\t#include \n\t#endif\n\t```', 'HELLO_WORLD_ROBOT_CREATION': '\t```cpp\n\tauto robot = std::make_shared("pexod.urdf");\n\t```', 'HELLO_WORLD_ROBOT_PLACING': '\t```cpp\n\trobot->set_base_pose(robot_dart::make_tf({0., 0., 0.2}));\n\t```', 'HELLO_WORLD_ROBOT_SIMU': '\t```cpp\n\trobot_dart::RobotDARTSimu simu(0.001); // dt=0.001, 1KHz simulation\n\tsimu.add_floor();\n\tsimu.add_robot(robot);\n\t```', 'HELLO_WORLD_ROBOT_GRAPHIC': '\t```cpp\n\tauto graphics = std::make_shared();\n\tsimu.set_graphics(graphics);\n\tgraphics->look_at({0.5, 3., 0.75}, {0.5, 0., 0.2});\n\t```', 'HELLO_WORLD_ROBOT_RUN': '\t```cpp\n\tsimu.run(10.);\n\t```', 'TALOS_FAST': '\t```cpp\n\t// load talos fast\n\tauto robot = std::make_shared();\n\t```', 'ADD_NEW_CAMERA': '\t```cpp\n\t// Add camera\n\tauto camera = std::make_shared(graphics->magnum_app(), 256, 256);\n\t```', 'MANIPULATE_CAM_SEP': '\t```cpp\n\tcamera->camera().set_far_plane(5.f);\n\tcamera->camera().set_near_plane(0.01f);\n\tcamera->camera().set_fov(60.0f);\n\t```', 'MANIPULATE_CAM': '\t```cpp\n\tcamera->camera().set_camera_params(5., // far plane\n\t 0.01f, // near plane\n\t 60.0f, // field of view\n\t 600, // width\n\t 400 // height\n\t);\n\t```', 'RECORD_VIDEO_CAMERA': '\t```cpp\n\t// cameras can also record video\n\tcamera->record_video("video-camera.mp4");\n\t```', 'CAM_POSITION': '\t```cpp\n\t// set the position of the camera, and the position where the main camera is looking at\n\tEigen::Vector3d cam_pos = {-0.5, -3., 0.75};\n\tEigen::Vector3d cam_looks_at = {0.5, 0., 0.2};\n\tcamera->look_at(cam_pos, cam_looks_at);\n\t```', 'CAM_ATTACH': '\t```cpp\n\tEigen::Isometry3d tf;\n\ttf = Eigen::AngleAxisd(3.14, Eigen::Vector3d{1., 0., 0.});\n\ttf *= Eigen::AngleAxisd(1.57, Eigen::Vector3d{0., 0., 1.});\n\ttf.translation() = Eigen::Vector3d(0., 0., 0.1);\n\tcamera->attach_to_body(robot->body_node("iiwa_link_ee"), tf); // cameras are looking towards -z by default\n\t```', 'ROBOT_POOL_INCLUDE': '\t```cpp\n\t#include \n\t```', 'ROBOT_POOL_GLOBAL_NAMESPACE': '\t```cpp\n\tnamespace pool {\n\t // This function should load one robot: here we load Talos\n\t inline std::shared_ptr robot_creator()\n\t {\n\t return std::make_shared();\n\t }\n\t\n\t // To create the object we need to pass the robot_creator function and the number of maximum parallel threads\n\t robot_dart::RobotPool robot_pool(robot_creator, NUM_THREADS);\n\t} // namespace pool\n\t```', 'ROBOT_POOL_EVAL': '\t```cpp\n\tinline void eval_robot(int i)\n\t{\n\t // We get one available robot\n\t auto robot = pool::robot_pool.get_robot();\n\t std::cout << "Robot " << i << " got [" << robot->skeleton() << "]" << std::endl;\n\t\n\t /// --- some robot_dart code ---\n\t simulate_robot(robot);\n\t // --- do something with the result\n\t\n\t std::cout << "End of simulation " << i << std::endl;\n\t\n\t // CRITICAL : free your robot !\n\t pool::robot_pool.free_robot(robot);\n\t\n\t std::cout << "Robot " << i << " freed!" << std::endl;\n\t}\n\t```', 'ROBOT_POOL_CREATE_THREADS': '\t```cpp\n\t// for the example, we run NUM_THREADS threads of eval_robot()\n\tstd::vector threads(NUM_THREADS * 2); // *2 to see some reuse\n\tfor (size_t i = 0; i < threads.size(); ++i)\n\t threads[i] = std::thread(eval_robot, i); // eval_robot is the function that uses the robot\n\t```', 'KINEMATICS': '\t```cpp\n\t// Get Joint Positions(Angles)\n\tauto joint_positions = robot->positions();\n\t\n\t// Get Joint Velocities\n\tauto joint_vels = robot->velocities();\n\t\n\t// Get Joint Accelerations\n\tauto joint_accs = robot->accelerations();\n\t\n\t// Get link_name(str) Transformation matrix with respect to the world frame.\n\tauto eef_tf = robot->body_pose(link_name);\n\t\n\t// Get translation vector from transformation matrix\n\tauto eef_pos = eef_tf.translation();\n\t\n\t// Get rotation matrix from tranformation matrix\n\tauto eef_rot = eef_tf.rotation();\n\t\n\t// Get link_name 6d pose vector [logmap(eef_tf.linear()), eef_tf.translation()]\n\tauto eef_pose_vec = robot->body_pose_vec(link_name);\n\t\n\t// Get link_name 6d velocity vector [angular, cartesian]\n\tauto eef_vel = robot->body_velocity(link_name);\n\t\n\t// Get link_name 6d acceleration vector [angular, cartesian]\n\tauto eef_acc = robot->body_acceleration(link_name);\n\t\n\t// Jacobian targeting the origin of link_name(str)\n\tauto jacobian = robot->jacobian(link_name);\n\t\n\t// Jacobian time derivative\n\tauto jacobian_deriv = robot->jacobian_deriv(link_name);\n\t\n\t// Center of Mass Jacobian\n\tauto com_jacobian = robot->com_jacobian();\n\t\n\t// Center of Mass Jacobian Time Derivative\n\tauto com_jacobian_deriv = robot->com_jacobian_deriv();\n\t```', 'DYNAMICS': "\t```cpp\n\t// Get Joint Forces\n\tauto joint_forces = robot->forces();\n\t\n\t// Get link's mass\n\tauto eef_mass = robot->body_mass(link_name);\n\t\n\t// Mass Matrix of robot\n\tauto mass_matrix = robot->mass_matrix();\n\t\n\t// Inverse of Mass Matrix\n\tauto inv_mass_matrix = robot->inv_mass_matrix();\n\t\n\t// Augmented Mass matrix\n\tauto aug_mass_matrix = robot->aug_mass_matrix();\n\t\n\t// Inverse of Augmented Mass matrix\n\tauto inv_aug_mass_matrix = robot->inv_aug_mass_matrix();\n\t\n\t// Coriolis Force vector\n\tauto coriolis = robot->coriolis_forces();\n\t\n\t// Gravity Force vector\n\tauto gravity = robot->gravity_forces();\n\t\n\t// Combined vector of Coriolis Force and Gravity Force\n\tauto coriolis_gravity = robot->coriolis_gravity_forces();\n\t\n\t// Constraint Force Vector\n\tauto constraint_forces = robot->constraint_forces();\n\t```", 'IIWA': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'ROBOT_GHOST': '\t```cpp\n\t// Add a ghost robot; only visuals, no dynamics, no collision\n\tauto ghost = robot->clone_ghost();\n\tsimu.add_robot(ghost);\n\t```', 'LOAD_IICUB_PYTHON': '\t```python\n\trobot = rd.ICub()\n\t\n\t# Set actuator types to VELOCITY motors so that they stay in position without any controller\n\trobot.set_actuator_types("velocity")\n\tsimu = rd.RobotDARTSimu(0.001)\n\tsimu.set_collision_detector("fcl")\n\t```', 'ICUB_PRINT_IMU_PYTHON': '\t```python\n\tprint("Angular Position: ", robot.imu().angular_position_vec().transpose())\n\tprint("Angular Velocity: ", robot.imu().angular_velocity().transpose())\n\tprint("Linear Acceleration: ", robot.imu().linear_acceleration().transpose())\n\tprint("=================================" )\n\t```', 'ICUB_PRINT_FT_PYTHON': '\t```python\n\tprint("FT ( force): ", robot.ft_foot_left().force().transpose())\n\tprint("FT (torque): ", robot.ft_foot_left().torque().transpose())\n\tprint("=================================")\n\t```', 'CAMERAS_PARALLEL_PYTHON': '\t```python\n\trobot = rd.Robot("arm.urdf", "arm", False)\n\trobot.fix_to_world()\n\t\n\tdef test():\n\t pid = os.getpid()\n\t ii = pid%15\n\t\n\t # create the controller\n\t pdcontrol = rd.PDControl([0.0, 1.0, -1.5, 1.0], False)\n\t\n\t # clone the robot\n\t grobot = robot.clone()\n\t\n\t # add the controller to the robot\n\t grobot.add_controller(pdcontrol, 1.)\n\t pdcontrol.set_pd(200., 20.)\n\t\n\t # create the simulation object\n\t simu = rd.RobotDARTSimu(0.001)\n\t\n\t # set the graphics\n\t graphics = rd.gui.WindowlessGraphics()\n\t simu.set_graphics(graphics)\n\t\n\t graphics.look_at([0.4 * ii, 3.5 - ii * 0.1, 2.], [0., 0., 0.25], [0., 0., 1.])\n\t\n\t # add the robot and the floor\n\t simu.add_robot(grobot)\n\t simu.add_checkerboard_floor()\n\t\n\t # run\n\t simu.run(20.)\n\t\n\t # save last frame for visualization purposes\n\t img = graphics.image()\n\t rd.gui.save_png_image(\'camera-\' + str(ii) + \'.png\', img)\n\t\n\t# helper function to run in parallel\n\tdef runInParallel(N):\n\t proc = []\n\t for i in range(N):\n\t # rd.gui.run_with_gl_context accepts 2 parameters:\n\t # (func, wait_time_in_ms)\n\t # the func needs to be of the following format: void(), aka no return, no arguments\n\t p = Process(target=rd.gui.run_with_gl_context, args=(test, 20))\n\t p.start()\n\t proc.append(p)\n\t for p in proc:\n\t p.join()\n\t\n\tprint(\'Running parallel evaluations\')\n\tN = 15\n\tstart = timer()\n\trunInParallel(N)\n\tend = timer()\n\tprint(\'Time:\', end-start)\n\t```', 'SIMPLE_CONTROL_PYTHON': '\t```python\n\tcontroller1 = rd.SimpleControl(ctrl)\n\trobot.add_controller(controller1)\n\t```', 'INIT_SIMU_PYTHON': '\t```python\n\t# choose time step of 0.001 seconds\n\tsimu = rd.RobotDARTSimu(0.001)\n\t```', 'MODIFY_SIMU_DT_PYTHON': '\t```python\n\t# set timestep to 0.005 and update control frequency(bool)\n\tsimu.set_timestep(0.005, True)\n\t```', 'SIMU_GRAVITY_PYTHON': '\t```python\n\t# set gravitational force of mars\n\tmars_gravity = [0., 0., -3.721]\n\tsimu.set_gravity(mars_gravity)\n\t```', 'GRAPHICS_PARAMS_PYTHON': '\t```python\n\tconfiguration = rd.gui.GraphicsConfiguration()\n\t# We can change the width/height of the window (or camera, dimensions)\n\tconfiguration.width = 1280\n\tconfiguration.height = 960\n\tconfiguration.title = "Graphics Tutorial" # We can set a title for our window\n\t\n\t# We can change the configuration for shadows\n\tconfiguration.shadowed = True\n\tconfiguration.transparent_shadows = True\n\tconfiguration.shadow_map_size = 1024\n\t\n\t# We can also alter some specifications for the lighting\n\tconfiguration.max_lights = 3 # maximum number of lights for our scene\n\tconfiguration.specular_strength = 0.25 # strength og the specular component\n\t\n\t# Some extra configuration for the main camera\n\tconfiguration.draw_main_camera = True\n\tconfiguration.draw_debug = True\n\tconfiguration.draw_text = True\n\t\n\t# We can also change the background color [default = black]\n\tconfiguration.bg_color = [1., 1., 1., 1.]\n\t\n\t# create the graphics object with the configuration as a parameter\n\tgraphics = rd.gui.Graphics(configuration)\n\t```', 'SHADOWS_GRAPHICS_PYTHON': '\t```python\n\t# Disable shadows\n\tgraphics.enable_shadows(False, False)\n\tsimu.run(1.)\n\t# Enable shadows only for non-transparent objects\n\tgraphics.enable_shadows(True, False)\n\tsimu.run(1.)\n\t# Enable shadows for transparent objects as well\n\tgraphics.enable_shadows(True, True)\n\tsimu.run(1.)\n\t```', 'CLR_LIGHT_PYTHON': '\t```python\n\t# Clear Lights\n\tgraphics.clear_lights()\n\t```', 'LIGHT_MATERIAL_PYTHON': '\t```python\n\t# Clear Light material\n\tshininess = 1000.\n\twhite = magnum.Color4(1., 1., 1., 1.)\n\t\n\t# ambient, diffuse, specular\n\tcustom_material = rd.gui.Material(white, white, white, shininess)\n\t```', 'POINT_LIGHT_PYTHON': '\t```python\n\t# Create point light\n\tposition = magnum.Vector3(0., 0., 2.)\n\tintensity = 1.\n\tattenuation_terms = magnum.Vector3(1., 0., 0.)\n\tpoint_light = rd.gui.create_point_light(position, custom_material, intensity, attenuation_terms)\n\tgraphics.add_light(point_light)\n\t```', 'DIRECTIONAL_LIGHT_PYTHON': '\t```python\n\t# Create directional light\n\tdirection = magnum.Vector3(-1, -1, -1)\n\tdirectional_light = rd.gui.create_directional_light(direction, custom_material)\n\tgraphics.add_light(directional_light)\n\t```', 'SPOT_LIGHT_PYTHON': '\t```python\n\t# Create spot light\n\tposition = magnum.Vector3(0., 0., 1.)\n\tdirection = magnum.Vector3(-1, -1, -1)\n\tintensity = 1.\n\tattenuation_terms = magnum.Vector3(1., 0., 0.)\n\tspot_exponent = np.pi\n\tspot_cut_off = np.pi / 8\n\tspot_light = rd.gui.create_spot_light(position, custom_material, direction, spot_exponent, spot_cut_off, intensity, attenuation_terms)\n\tgraphics.add_light(spot_light)\n\t```', 'SET_COLLISION_DETECTOR_PYTHON': '\t```python\n\tsimu.set_collision_detector("fcl") # collision_detector can be "dart", "fcl", "ode" or "bullet" (case does not matter)\n\t```', 'SELF_COLLISIONS_PYTHON': '\t```python\n\tif(not robot.self_colliding()):\n\t print("Self collision is not enabled")\n\t # set self collisions to true and adjacent collisions to false\n\t robot.set_self_collision(True, False)\n\t```', 'ROBOT_CONTROL_PYTHON': '\t```python\n\tclass MyController(rd.RobotControl):\n\t def __init__(self, ctrl, full_control):\n\t rd.RobotControl.__init__(self, ctrl, full_control)\n\t\n\t def __init__(self, ctrl, controllable_dofs):\n\t rd.RobotControl.__init__(self, ctrl, controllable_dofs)\n\t\n\t def configure(self):\n\t self._active = True\n\t\n\t def calculate(self, t):\n\t target = np.array([self._ctrl])\n\t cmd = 100*(target-self.robot().positions(self._controllable_dofs))\n\t return cmd[0]\n\t\n\t # TO-DO: This is NOT working at the moment!\n\t def clone(self):\n\t return MyController(self._ctrl, self._controllable_dofs)\n\t```', 'SET_ACTUATOR_PYTHON': '\t```python\n\t# Set all DoFs to same actuator\n\t# actuator types can be "servo", "torque", "velocity", "passive", "locked", "mimic"\n\trobot.set_actuator_types("servo")\n\t# You can also set actuator types separately\n\trobot.set_actuator_type("torque", "iiwa_joint_5")\n\t```', 'POSITIONS_ENFORCED_PYTHON': '\t```python\n\t# Εnforce joint position and velocity limits\n\trobot.set_position_enforced(True)\n\t```', 'MODIFY_LIMITS_PYTHON': '\t```python\n\t# Modify Position Limits\n\tpos_upper_lims = np.array([2.096, 2.096, 2.096, 2.096, 2.096, 2.096, 2.096])\n\trobot.set_position_upper_limits(pos_upper_lims, dof_names)\n\trobot.set_position_lower_limits(-1*pos_upper_lims, dof_names)\n\t\n\t# Modify Velocity Limits\n\tvel_upper_lims = np.array([1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5])\n\t\n\trobot.set_velocity_upper_limits(vel_upper_lims, dof_names)\n\trobot.set_velocity_lower_limits(-1*vel_upper_lims, dof_names)\n\t\n\t# Modify Force Limits\n\tforce_upper_lims = np.array([150, 150, 150, 150, 150, 150, 150])\n\trobot.set_force_upper_limits(force_upper_lims, dof_names)\n\trobot.set_force_lower_limits(-1*force_upper_lims, dof_names)\n\t\n\t# Modify Acceleration Limits\n\tacc_upper_lims = np.array([1500, 1500, 1500, 1500, 1500, 1500, 1500])\n\trobot.set_acceleration_upper_limits(acc_upper_lims, dof_names)\n\trobot.set_acceleration_lower_limits(-1*acc_upper_lims, dof_names)\n\t```', 'MODIFY_COEFFS_PYTHON': '\t```python\n\t# Modify Damping Coefficients\n\tdamps = [0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4]\n\trobot.set_damping_coeffs(damps, dof_names)\n\t\n\t# Modify Coulomb Frictions\n\tcfrictions = [0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001]\n\trobot.set_coulomb_coeffs(cfrictions, dof_names)\n\t\n\t# Modify Spring Stiffness\n\tstiffnesses = [0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001]\n\trobot.set_spring_stiffnesses(stiffnesses, dof_names)\n\t```', 'IIWA_PYTHON': '\t```python\n\trobot = rd.Iiwa()\n\t```', 'ROBOT_GHOST_PYTHON': '\t```python\n\t# Add a ghost robot; only visuals, no dynamics, no collision\n\tghost = robot.clone_ghost()\n\tsimu.add_robot(ghost)\n\t```', 'HEXAPOD_PYTHON': '\t```python\n\trobot = rd.Hexapod()\n\t```', 'TALOS_FAST_PYTHON': '\t```python\n\trobot = rd.TalosFastCollision()\n\t```', 'RECORD_VIDEO_ROBOT_GRAPHICS_PARAMS_PYTHON': '\t```python\n\tgraphics.record_video("talos_dancing.mp4")\n\t```', 'SIMPLE_ARM_PYTHON': '\t```python\n\trobot = rd.Arm()\n\t```', 'A1_PYTHON': '\t```python\n\trobot = rd.A1()\n\t```', 'A1_PRINT_IMU_PYTHON': '\t```python\n\tprint( "Angular Position: ", robot.imu().angular_position_vec().transpose())\n\tprint( "Angular Velocity: ", robot.imu().angular_velocity().transpose())\n\tprint( "Linear Acceleration: ", robot.imu().linear_acceleration().transpose())\n\tprint( "=================================")\n\t```', 'CAMERA_SENSOR_RGBD_RECORD_DEPTH_PYTHON': '\t```python\n\t# cameras are recording color images by default, enable depth images as well for this example\n\tcamera.camera().record(True, True)\n\t```', 'TORQUE_SENSOR_PYTHON': '\t```python\n\t# Add torque sensors to the robot\n\ttq_sensors = np.empty(robot.num_dofs(), dtype=rd.sensor.Torque)\n\tct = 0\n\tfor joint in robot.dof_names():\n\t simu.add_sensor(rd.sensor.Torque(robot, joint, 1000))\n\t tq_sensors[ct] = simu.sensors()[-1]\n\t ct += 1\n\t```', 'FORCE_TORQUE_SENSOR_PYTHON': '\t```python\n\t# Add force-torque sensors to the robot\n\tf_tq_sensors = np.empty(robot.num_dofs(), dtype=rd.sensor.ForceTorque)\n\tct = 0\n\tfor joint in robot.dof_names():\n\t simu.add_sensor(rd.sensor.ForceTorque(\n\t robot, joint, 1000, "parent_to_child"))\n\t f_tq_sensors[ct] = simu.sensors()[-1]\n\t print(f_tq_sensors)\n\t ct += 1\n\t```', 'IMU_SENSOR_PYTHON': '\t```python\n\t# Add IMU sensors to the robot\n\tct = 0\n\timu_sensors = np.empty(robot.num_bodies(), dtype=rd.sensor.IMU)\n\tfor body_node in robot.body_names():\n\t simu.add_sensor(rd.sensor.IMU(\n\t rd.sensor.IMUConfig(robot.body_node(body_node), 1000)))\n\t imu_sensors[ct] = simu.sensors()[-1]\n\t ct += 1\n\t```', 'TORQUE_MEASUREMENT_PYTHON': '\t```python\n\t# vector that contains the torque measurement for every joint (scalar)\n\ttorques_measure = np.empty(robot.num_dofs())\n\tct = 0\n\tfor tq_sens in tq_sensors:\n\t torques_measure[ct] = tq_sens.torques()\n\t ct += 1\n\t```', 'FORCE_TORQUE_MEASUREMENT_PYTHON': '\t```python\n\t# matrix that contains the torque measurement for every joint (3d vector)\n\tft_torques_measure = np.empty([robot.num_dofs(), 3])\n\t# matrix that contains the force measurement for every joint (3d vector)\n\tft_forces_measure = np.empty([robot.num_dofs(), 3])\n\t# matrix that contains the wrench measurement for every joint (6d vector)[torque, force]\n\tft_wrench_measure = np.empty([robot.num_dofs(), 6])\n\tct = 0\n\tfor f_tq_sens in f_tq_sensors:\n\t ft_torques_measure[ct, :] = f_tq_sens.torque()\n\t ft_forces_measure[ct, :] = f_tq_sens.force()\n\t ft_wrench_measure[ct, :] = f_tq_sens.wrench()\n\t ct += 1\n\t\n\t```', 'IMU_MEASUREMENT_PYTHON': '\t```python\n\timu_angular_positions_measure = np.empty([robot.num_bodies(), 3])\n\timu_angular_velocities_measure = np.empty([robot.num_bodies(), 3])\n\timu_linear_acceleration_measure = np.empty([robot.num_bodies(), 3])\n\tct = 0\n\tfor imu_sens in imu_sensors:\n\t imu_angular_positions_measure[ct,:] = imu_sens.angular_position_vec()\n\t imu_angular_velocities_measure[ct, :] = imu_sens.angular_velocity()\n\t imu_linear_acceleration_measure[ct,:] = imu_sens.linear_acceleration()\n\t ct += 1\n\t\n\t```', 'RGB_SENSOR_PYTHON': '\t```python\n\t# a nested array (w*h*3) of the last image taken can be retrieved\n\trgb_image = camera.image()\n\t```', 'RGB_SENSOR_MEASURE_PYTHON': '\t```python\n\t# we can also save them to png\n\trd.gui.save_png_image("camera-small.png", rgb_image)\n\t# convert an rgb image to grayscale (useful in some cases)\n\tgray_image = rd.gui.convert_rgb_to_grayscale(rgb_image)\n\trd.gui.save_png_image("camera-gray.png", gray_image)\n\t```', 'RGB_D_SENSOR_PYTHON': '\t```python\n\t# get the depth image from a camera\n\t# with a version for visualization or bigger differences in the output\n\trgb_d_image = camera.depth_image()\n\t# and the raw values that can be used along with the camera parameters to transform the image to point-cloud\n\trgb_d_image_raw = camera.raw_depth_image()\n\t```', 'RGB_D_SENSOR_MEASURE_PYTHON': '\t```python\n\trd.gui.save_png_image("camera-depth.png", rgb_d_image)\n\trd.gui.save_png_image("camera-depth-raw.png", rgb_d_image_raw)\n\t```', 'FRANKA_PYTHON': '\t```python\n\trobot = rd.Franka()\n\t```', 'PD_CONTROL_PYTHON': '\t```python\n\t# add a PD-controller to the arm\n\t# set desired positions\n\tctrl = [0., np.pi / 4., 0., -np.pi / 4., 0., np.pi / 2., 0., 0.]\n\t\n\t# add the controller to the robot\n\tcontroller = rd.PDControl(ctrl)\n\trobot.add_controller(controller)\n\tcontroller.set_pd(300., 50.)\n\t```', 'TALOS_PYTHON': '\t```python\n\trobot = rd.Talos()\n\t```', 'KINEMATICS_PYTHON': '\t```python\n\t# Get Joint Positions(Angles)\n\tjoint_positions = robot.positions()\n\t\n\t# Get Joint Velocities\n\tjoint_vels = robot.velocities()\n\t\n\t# Get Joint Accelerations\n\tjoint_accs = robot.accelerations()\n\t\n\t# Get link_name(str) Transformation matrix with respect to the world frame.\n\teef_tf = robot.body_pose(link_name)\n\t\n\t# Get translation vector from transformation matrix\n\teef_pos = eef_tf.translation()\n\t\n\t# Get rotation matrix from tranformation matrix\n\teef_rot = eef_tf.rotation()\n\t\n\t# Get link_name 6d pose vector [logmap(eef_tf.linear()), eef_tf.translation()]\n\teef_pose_vec = robot.body_pose_vec(link_name)\n\t\n\t# Get link_name 6d velocity vector [angular, cartesian]\n\teef_vel = robot.body_velocity(link_name)\n\t\n\t# Get link_name 6d acceleration vector [angular, cartesian]\n\teef_acc = robot.body_acceleration(link_name)\n\t\n\t# Jacobian targeting the origin of link_name(str)\n\tjacobian = robot.jacobian(link_name)\n\t\n\t# Jacobian time derivative\n\tjacobian_deriv = robot.jacobian_deriv(link_name)\n\t\n\t# Center of Mass Jacobian\n\tcom_jacobian = robot.com_jacobian()\n\t\n\t# Center of Mass Jacobian Time Derivative\n\tcom_jacobian_deriv = robot.com_jacobian_deriv()\n\t```', 'DYNAMICS_PYTHON': "\t```python\n\t# Get Joint Forces\n\tjoint_forces = robot.forces()\n\t\n\t# Get link's mass\n\teef_mass = robot.body_mass(link_name)\n\t\n\t# Mass Matrix of robot\n\tmass_matrix = robot.mass_matrix()\n\t\n\t# Inverse of Mass Matrix\n\tinv_mass_matrix = robot.inv_mass_matrix()\n\t\n\t# Augmented Mass matrix\n\taug_mass_matrix = robot.aug_mass_matrix()\n\t\n\t# Inverse of Augmented Mass matrix\n\tinv_aug_mass_matrix = robot.inv_aug_mass_matrix()\n\t\n\t# Coriolis Force vector\n\tcoriolis = robot.coriolis_forces()\n\t\n\t# Gravity Force vector\n\tgravity = robot.gravity_forces()\n\t\n\t# Combined vector of Coriolis Force and Gravity Force\n\tcoriolis_gravity = robot.coriolis_gravity_forces()\n\t\n\t# NOT IMPLEMENTED\n\t# # Constraint Force Vector\n\t# constraint_forces = robot.constraint_forces()\n\t\n\t```", 'ADD_NEW_CAMERA_PYTHON': '\t```python\n\t# Add camera\n\tcamera = rd.sensor.Camera(graphics.magnum_app(), 32, 32)\n\t```', 'MANIPULATE_CAM_SEP_PYTHON': '\t```python\n\tcamera.camera().set_far_plane(5.)\n\tcamera.camera().set_near_plane(0.01)\n\tcamera.camera().set_fov(60.0)\n\t```', 'MANIPULATE_CAM_PYTHON': '\t```python\n\tcamera.camera().set_camera_params(5., #far plane\n\t 0.01, #near plane\n\t 60.0, # field of view\n\t 600, # width\n\t 400) #height\n\t```', 'RECORD_VIDEO_CAMERA_PYTHON': '\t```python\n\t\n\t# cameras can also record video\n\tcamera.record_video("video-camera.mp4")\n\t```', 'CAM_POSITION_PYTHON': '\t```python\n\t# set the position of the camera, and the position where the main camera is looking at\n\tcam_pos = [-0.5, -3., 0.75]\n\tcam_looks_at = [0.5, 0., 0.2]\n\tcamera.look_at(cam_pos, cam_looks_at)\n\t```', 'CAM_ATTACH_PYTHON': '\t```python\n\ttf = dartpy.math.Isometry3()\n\trot = dartpy.math.AngleAxis(3.14, [1., 0., 0.])\n\trot = rot.multiply( dartpy.math.AngleAxis(1.57, [0., 0., 1.])).to_rotation_matrix()\n\ttf.set_translation([0., 0., 0.1])\n\tcamera.attach_to_body(robot.body_node("iiwa_link_ee"), tf) # cameras are looking towards -z by default\n\t```', 'HELLO_WORLD_INCLUDE_PYTHON': '\t```python\n\timport RobotDART as rd\n\t```', 'HELLO_WORLD_ROBOT_CREATION_PYTHON': '\t```python\n\trobot = rd.Robot("pexod.urdf");\n\t```', 'HELLO_WORLD_ROBOT_PLACING_PYTHON': '\t```python\n\trobot.set_base_pose([0., 0., 0., 0., 0., 0.2])\n\t```', 'HELLO_WORLD_ROBOT_SIMU_PYTHON': '\t```python\n\tsimu = rd.RobotDARTSimu(0.001); # dt=0.001, 1KHz simulation\n\tsimu.add_floor();\n\tsimu.add_robot(robot);\n\t```', 'HELLO_WORLD_ROBOT_GRAPHIC_PYTHON': '\t```python\n\tgraphics = rd.gui.Graphics()\n\tsimu.set_graphics(graphics)\n\tgraphics.look_at([0.5, 3., 0.75], [0.5, 0., 0.2])\n\t```', 'HELLO_WORLD_ROBOT_RUN_PYTHON': '\t```python\n\tsimu.run(10.)\n\t```'} + variables = {'TALOS_FAST': '\t```cpp\n\t// load talos fast\n\tauto robot = std::make_shared();\n\t```', 'A1': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'A1_PRINT_IMU': '\t```cpp\n\tstd::cout << "Angular Position: " << robot->imu().angular_position_vec().transpose().format(fmt) << std::endl;\n\tstd::cout << "Angular Velocity: " << robot->imu().angular_velocity().transpose().format(fmt) << std::endl;\n\tstd::cout << "Linear Acceleration: " << robot->imu().linear_acceleration().transpose().format(fmt) << std::endl;\n\tstd::cout << "=================================" << std::endl;\n\t```', 'FRANKA': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'PD_CONTROL': '\t```cpp\n\t// add a PD-controller to the arm\n\t// set desired positions\n\tEigen::VectorXd ctrl = robot_dart::make_vector({0., M_PI / 4., 0., -M_PI / 4., 0., M_PI / 2., 0., 0.});\n\t\n\t// add the controller to the robot\n\tauto controller = std::make_shared(ctrl);\n\trobot->add_controller(controller);\n\tcontroller->set_pd(300., 50.);\n\t```', 'IIWA': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'ROBOT_GHOST': '\t```cpp\n\t// Add a ghost robot; only visuals, no dynamics, no collision\n\tauto ghost = robot->clone_ghost();\n\tsimu.add_robot(ghost);\n\t```', 'CAMERAS_PARALLEL': '\t```cpp\n\t// Load robot from URDF\n\tauto global_robot = std::make_shared();\n\t\n\tstd::vector workers;\n\t\n\t// Set maximum number of parallel GL contexts (this is GPU-dependent)\n\trobot_dart::gui::magnum::GlobalData::instance()->set_max_contexts(4);\n\t\n\t// We want 15 parallel simulations with different GL context each\n\tsize_t N_workers = 15;\n\tstd::mutex mutex;\n\tsize_t id = 0;\n\t// Launch the workers\n\tfor (size_t i = 0; i < N_workers; i++) {\n\t workers.push_back(std::thread([&] {\n\t mutex.lock();\n\t size_t index = id++;\n\t mutex.unlock();\n\t\n\t // Get the GL context -- this is a blocking call\n\t // will wait until one GL context is available\n\t // get_gl_context(gl_context); // this call will not sleep between failed queries\n\t get_gl_context_with_sleep(gl_context, 20); // this call will sleep 20ms between each failed query\n\t\n\t // Do the simulation\n\t auto robot = global_robot->clone();\n\t\n\t robot_dart::RobotDARTSimu simu(0.001);\n\t\n\t Eigen::VectorXd ctrl = robot_dart::make_vector({0., M_PI / 3., 0., -M_PI / 4., 0., 0., 0.});\n\t\n\t auto controller = std::make_shared(ctrl);\n\t robot->add_controller(controller);\n\t controller->set_pd(300., 50.);\n\t\n\t // Magnum graphics\n\t robot_dart::gui::magnum::GraphicsConfiguration configuration = robot_dart::gui::magnum::WindowlessGraphics::default_configuration();\n\t\n\t configuration.width = 1024;\n\t configuration.height = 768;\n\t auto graphics = std::make_shared(configuration);\n\t simu.set_graphics(graphics);\n\t // Position the camera differently for each thread to visualize the difference\n\t graphics->look_at({0.4 * index, 3.5 - index * 0.1, 2.}, {0., 0., 0.25});\n\t // record images from main camera/graphics\n\t // graphics->set_recording(true); // WindowlessGLApplication records images by default\n\t\n\t simu.add_robot(robot);\n\t simu.run(6);\n\t\n\t // Save the image for verification\n\t robot_dart::gui::save_png_image("camera_" + std::to_string(index) + ".png",\n\t graphics->image());\n\t\n\t // Release the GL context for another thread to use\n\t release_gl_context(gl_context);\n\t }));\n\t}\n\t\n\t// Wait for all the workers\n\tfor (size_t i = 0; i < workers.size(); i++) {\n\t workers[i].join();\n\t}\n\t```', 'INIT_SIMU': '\t```cpp\n\t// choose time step of 0.001 seconds\n\trobot_dart::RobotDARTSimu simu(0.001);\n\t```', 'MODIFY_SIMU_DT': '\t```cpp\n\t// set timestep to 0.005 and update control frequency(bool)\n\tsimu.set_timestep(0.005, true);\n\t```', 'SIMU_GRAVITY': '\t```cpp\n\t// Set gravitational force of mars\n\tEigen::Vector3d mars_gravity = {0., 0., -3.721};\n\tsimu.set_gravity(mars_gravity);\n\t```', 'GRAPHICS_PARAMS': '\t```cpp\n\trobot_dart::gui::magnum::GraphicsConfiguration configuration;\n\t// We can change the width/height of the window (or camera image dimensions)\n\tconfiguration.width = 1280;\n\tconfiguration.height = 960;\n\tconfiguration.title = "Graphics Tutorial"; // We can set a title for our window\n\t\n\t// We can change the configuration for shadows\n\tconfiguration.shadowed = true;\n\tconfiguration.transparent_shadows = true;\n\tconfiguration.shadow_map_size = 1024;\n\t\n\t// We can also alter some specifications for the lighting\n\tconfiguration.max_lights = 3; // maximum number of lights for our scene [default=3]\n\tconfiguration.specular_strength = 0.25; // strength of the specular component\n\t\n\t// Some extra configuration for the main camera\n\tconfiguration.draw_main_camera = true;\n\tconfiguration.draw_debug = true;\n\tconfiguration.draw_text = true;\n\t\n\t// We can also change the background color [default=black]\n\tconfiguration.bg_color = Eigen::Vector4d{1.0, 1.0, 1.0, 1.0};\n\t\n\t// Create the graphics object with the configuration as parameter\n\tauto graphics = std::make_shared(configuration);\n\t```', 'SHADOWS_GRAPHICS': '\t```cpp\n\t// Disable shadows\n\tgraphics->enable_shadows(false, false);\n\tsimu.run(1.);\n\t// Enable shadows only for non-transparent objects\n\tgraphics->enable_shadows(true, false);\n\tsimu.run(1.);\n\t// Enable shadows for transparent objects as well\n\tgraphics->enable_shadows(true, true);\n\tsimu.run(1.);\n\t```', 'CLR_LIGHT': '\t```cpp\n\t// Clear Lights\n\tgraphics->clear_lights();\n\t```', 'LIGHT_MATERIAL': '\t```cpp\n\t// Create Light material\n\tMagnum::Float shininess = 1000.f;\n\tMagnum::Color4 white = {1.f, 1.f, 1.f, 1.f};\n\t\n\t// ambient, diffuse, specular\n\tauto custom_material = robot_dart::gui::magnum::gs::Material(white, white, white, shininess);\n\t```', 'POINT_LIGHT': '\t```cpp\n\t// Create point light\n\tMagnum::Vector3 position = {0.f, 0.f, 2.f};\n\tMagnum::Float intensity = 1.f;\n\tMagnum::Vector3 attenuation_terms = {1.f, 0.f, 0.f};\n\tauto point_light = robot_dart::gui::magnum::gs::create_point_light(position, custom_material, intensity, attenuation_terms);\n\tgraphics->add_light(point_light);\n\t```', 'DIRECTIONAL_LIGHT': '\t```cpp\n\t// Create directional light\n\tMagnum::Vector3 direction = {-1.f, -1.f, -1.f};\n\tauto directional_light = robot_dart::gui::magnum::gs::create_directional_light(direction, custom_material);\n\tgraphics->add_light(directional_light);\n\t```', 'SPOT_LIGHT': '\t```cpp\n\t// Create spot light\n\tMagnum::Vector3 position = {0.f, 0.f, 1.f};\n\tMagnum::Vector3 direction = {-1.f, -1.f, -1.f};\n\tMagnum::Float intensity = 1.f;\n\tMagnum::Vector3 attenuation_terms = {1.f, 0.f, 0.f};\n\tMagnum::Float spot_exponent = M_PI;\n\tMagnum::Float spot_cut_off = M_PI / 8;\n\tauto spot_light = robot_dart::gui::magnum::gs::create_spot_light(position, custom_material, direction, spot_exponent, spot_cut_off, intensity, attenuation_terms);\n\tgraphics->add_light(spot_light);\n\t```', 'ADD_NEW_CAMERA': '\t```cpp\n\t// Add camera\n\tauto camera = std::make_shared(graphics->magnum_app(), 256, 256);\n\t```', 'MANIPULATE_CAM_SEP': '\t```cpp\n\tcamera->camera().set_far_plane(5.f);\n\tcamera->camera().set_near_plane(0.01f);\n\tcamera->camera().set_fov(60.0f);\n\t```', 'MANIPULATE_CAM': '\t```cpp\n\tcamera->camera().set_camera_params(5., // far plane\n\t 0.01f, // near plane\n\t 60.0f, // field of view\n\t 600, // width\n\t 400 // height\n\t);\n\t```', 'RECORD_VIDEO_CAMERA': '\t```cpp\n\t// cameras can also record video\n\tcamera->record_video("video-camera.mp4");\n\t```', 'CAM_POSITION': '\t```cpp\n\t// set the position of the camera, and the position where the main camera is looking at\n\tEigen::Vector3d cam_pos = {-0.5, -3., 0.75};\n\tEigen::Vector3d cam_looks_at = {0.5, 0., 0.2};\n\tcamera->look_at(cam_pos, cam_looks_at);\n\t```', 'CAM_ATTACH': '\t```cpp\n\tEigen::Isometry3d tf;\n\ttf = Eigen::AngleAxisd(3.14, Eigen::Vector3d{1., 0., 0.});\n\ttf *= Eigen::AngleAxisd(1.57, Eigen::Vector3d{0., 0., 1.});\n\ttf.translation() = Eigen::Vector3d(0., 0., 0.1);\n\tcamera->attach_to_body(robot->body_node("iiwa_link_ee"), tf); // cameras are looking towards -z by default\n\t```', 'ROBOT_POOL_INCLUDE': '\t```cpp\n\t#include \n\t```', 'ROBOT_POOL_GLOBAL_NAMESPACE': '\t```cpp\n\tnamespace pool {\n\t // This function should load one robot: here we load Talos\n\t inline std::shared_ptr robot_creator()\n\t {\n\t return std::make_shared();\n\t }\n\t\n\t // To create the object we need to pass the robot_creator function and the number of maximum parallel threads\n\t robot_dart::RobotPool robot_pool(robot_creator, NUM_THREADS);\n\t} // namespace pool\n\t```', 'ROBOT_POOL_EVAL': '\t```cpp\n\tinline void eval_robot(int i)\n\t{\n\t // We get one available robot\n\t auto robot = pool::robot_pool.get_robot();\n\t std::cout << "Robot " << i << " got [" << robot->skeleton() << "]" << std::endl;\n\t\n\t /// --- some robot_dart code ---\n\t simulate_robot(robot);\n\t // --- do something with the result\n\t\n\t std::cout << "End of simulation " << i << std::endl;\n\t\n\t // CRITICAL : free your robot !\n\t pool::robot_pool.free_robot(robot);\n\t\n\t std::cout << "Robot " << i << " freed!" << std::endl;\n\t}\n\t```', 'ROBOT_POOL_CREATE_THREADS': '\t```cpp\n\t// for the example, we run NUM_THREADS threads of eval_robot()\n\tstd::vector threads(NUM_THREADS * 2); // *2 to see some reuse\n\tfor (size_t i = 0; i < threads.size(); ++i)\n\t threads[i] = std::thread(eval_robot, i); // eval_robot is the function that uses the robot\n\t```', 'TALOS': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'RECORD_VIDEO_ROBOT_GRAPHICS_PARAMS': '\t```cpp\n\tgraphics->record_video("talos_dancing.mp4");\n\t```', 'HELLO_WORLD_INCLUDE': '\t```cpp\n\t#include \n\t\n\t#ifdef GRAPHIC\n\t#include \n\t#endif\n\t```', 'HELLO_WORLD_ROBOT_CREATION': '\t```cpp\n\tauto robot = std::make_shared("pexod.urdf");\n\t```', 'HELLO_WORLD_ROBOT_PLACING': '\t```cpp\n\trobot->set_base_pose(robot_dart::make_tf({0., 0., 0.2}));\n\t```', 'HELLO_WORLD_ROBOT_SIMU': '\t```cpp\n\trobot_dart::RobotDARTSimu simu(0.001); // dt=0.001, 1KHz simulation\n\tsimu.add_floor();\n\tsimu.add_robot(robot);\n\t```', 'HELLO_WORLD_ROBOT_GRAPHIC': '\t```cpp\n\tauto graphics = std::make_shared();\n\tsimu.set_graphics(graphics);\n\tgraphics->look_at({0.5, 3., 0.75}, {0.5, 0., 0.2});\n\t```', 'HELLO_WORLD_ROBOT_RUN': '\t```cpp\n\tsimu.run(10.);\n\t```', 'LOAD_IICUB': '\t```cpp\n\tauto robot = std::make_shared();\n\t// Set actuator types to VELOCITY motors so that they stay in position without any controller\n\trobot->set_actuator_types("velocity");\n\trobot_dart::RobotDARTSimu simu(0.001);\n\tsimu.set_collision_detector("fcl");\n\t```', 'ICUB_PRINT_IMU': '\t```cpp\n\tstd::cout << "Angular Position: " << robot->imu().angular_position_vec().transpose().format(fmt) << std::endl;\n\tstd::cout << "Angular Velocity: " << robot->imu().angular_velocity().transpose().format(fmt) << std::endl;\n\tstd::cout << "Linear Acceleration: " << robot->imu().linear_acceleration().transpose().format(fmt) << std::endl;\n\tstd::cout << "=================================" << std::endl;\n\t```', 'ICUB_PRINT_FT': '\t```cpp\n\tstd::cout << "FT ( force): " << robot->ft_foot_left().force().transpose().format(fmt) << std::endl;\n\tstd::cout << "FT (torque): " << robot->ft_foot_left().torque().transpose().format(fmt) << std::endl;\n\tstd::cout << "=================================" << std::endl;\n\t```', 'SIMPLE_CONTROL': '\t```cpp\n\tauto controller1 = std::make_shared(ctrl);\n\t// add the controller to the robot, with a default weight of 1.0\n\trobot->add_controller(controller1);\n\t```', 'ROBOT_CONTROL': '\t```cpp\n\tclass MyController : public robot_dart::control::RobotControl {\n\tpublic:\n\t MyController() : robot_dart::control::RobotControl() {}\n\t\n\t MyController(const Eigen::VectorXd& ctrl, bool full_control) : robot_dart::control::RobotControl(ctrl, full_control) {}\n\t MyController(const Eigen::VectorXd& ctrl, const std::vector& dof_names) : robot_dart::control::RobotControl(ctrl, dof_names) {}\n\t\n\t void configure() override\n\t {\n\t _active = true;\n\t }\n\t\n\t Eigen::VectorXd calculate(double) override\n\t {\n\t auto robot = _robot.lock();\n\t Eigen::VectorXd cmd = 100. * (_ctrl - robot->positions(_controllable_dofs));\n\t return cmd;\n\t }\n\t std::shared_ptr clone() const override\n\t {\n\t return std::make_shared(*this);\n\t }\n\t};\n\t```', 'SIMPLE_ARM': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'KINEMATICS': '\t```cpp\n\t// Get Joint Positions(Angles)\n\tauto joint_positions = robot->positions();\n\t\n\t// Get Joint Velocities\n\tauto joint_vels = robot->velocities();\n\t\n\t// Get Joint Accelerations\n\tauto joint_accs = robot->accelerations();\n\t\n\t// Get link_name(str) Transformation matrix with respect to the world frame.\n\tauto eef_tf = robot->body_pose(link_name);\n\t\n\t// Get translation vector from transformation matrix\n\tauto eef_pos = eef_tf.translation();\n\t\n\t// Get rotation matrix from tranformation matrix\n\tauto eef_rot = eef_tf.rotation();\n\t\n\t// Get link_name 6d pose vector [logmap(eef_tf.linear()), eef_tf.translation()]\n\tauto eef_pose_vec = robot->body_pose_vec(link_name);\n\t\n\t// Get link_name 6d velocity vector [angular, cartesian]\n\tauto eef_vel = robot->body_velocity(link_name);\n\t\n\t// Get link_name 6d acceleration vector [angular, cartesian]\n\tauto eef_acc = robot->body_acceleration(link_name);\n\t\n\t// Jacobian targeting the origin of link_name(str)\n\tauto jacobian = robot->jacobian(link_name);\n\t\n\t// Jacobian time derivative\n\tauto jacobian_deriv = robot->jacobian_deriv(link_name);\n\t\n\t// Center of Mass Jacobian\n\tauto com_jacobian = robot->com_jacobian();\n\t\n\t// Center of Mass Jacobian Time Derivative\n\tauto com_jacobian_deriv = robot->com_jacobian_deriv();\n\t```', 'DYNAMICS': "\t```cpp\n\t// Get Joint Forces\n\tauto joint_forces = robot->forces();\n\t\n\t// Get link's mass\n\tauto eef_mass = robot->body_mass(link_name);\n\t\n\t// Mass Matrix of robot\n\tauto mass_matrix = robot->mass_matrix();\n\t\n\t// Inverse of Mass Matrix\n\tauto inv_mass_matrix = robot->inv_mass_matrix();\n\t\n\t// Augmented Mass matrix\n\tauto aug_mass_matrix = robot->aug_mass_matrix();\n\t\n\t// Inverse of Augmented Mass matrix\n\tauto inv_aug_mass_matrix = robot->inv_aug_mass_matrix();\n\t\n\t// Coriolis Force vector\n\tauto coriolis = robot->coriolis_forces();\n\t\n\t// Gravity Force vector\n\tauto gravity = robot->gravity_forces();\n\t\n\t// Combined vector of Coriolis Force and Gravity Force\n\tauto coriolis_gravity = robot->coriolis_gravity_forces();\n\t\n\t// Constraint Force Vector\n\tauto constraint_forces = robot->constraint_forces();\n\t```", 'CAMERA_SENSOR_RGBD_RECORD_DEPTH': '\t```cpp\n\tcamera->camera().record(true, true); // cameras are recording color images by default, enable depth images as well for this example\n\t```', 'TORQUE_SENSOR': '\t```cpp\n\t// Add torque sensors to the robot\n\tint ct = 0;\n\tstd::vector> tq_sensors(robot->num_dofs());\n\tfor (const auto& joint : robot->dof_names())\n\t tq_sensors[ct++] = simu.add_sensor(robot, joint, 1000);\n\t```', 'FORCE_TORQUE_SENSOR': '\t```cpp\n\t// Add force-torque sensors to the robot\n\tct = 0;\n\tstd::vector> f_tq_sensors(robot->num_dofs());\n\tfor (const auto& joint : robot->dof_names())\n\t f_tq_sensors[ct++] = simu.add_sensor(robot, joint, 1000, "parent_to_child");\n\t```', 'IMU_SENSOR': '\t```cpp\n\t// Add IMU sensors to the robot\n\tct = 0;\n\tstd::vector> imu_sensors(robot->num_bodies());\n\tfor (const auto& body_node : robot->body_names()) {\n\t // _imu(std::make_shared(sensor::IMUConfig(body_node("head"), frequency))),\n\t imu_sensors[ct++] = simu.add_sensor(robot_dart::sensor::IMUConfig(robot->body_node(body_node), 1000));\n\t}\n\t```', 'TORQUE_MEASUREMENT': '\t```cpp\n\t// vector that contains the torque measurement for every joint (scalar)\n\tEigen::VectorXd torques_measure(robot->num_dofs());\n\tfor (const auto& tq_sens : tq_sensors)\n\t torques_measure.block<1, 1>(ct++, 0) = tq_sens->torques();\n\t```', 'FORCE_TORQUE_MEASUREMENT': '\t```cpp\n\t// matrix that contains the torque measurement for every joint (3d vector)\n\tEigen::MatrixXd ft_torques_measure(robot->num_dofs(), 3);\n\t// matrix that contains the force measurement for every joint (3d vector)\n\tEigen::MatrixXd ft_forces_measure(robot->num_dofs(), 3);\n\t// matrix that contains the wrench measurement for every joint (6d vector)[torque, force]\n\tEigen::MatrixXd ft_wrench_measure(robot->num_dofs(), 6);\n\tct = 0;\n\tfor (const auto& f_tq_sens : f_tq_sensors) {\n\t ft_torques_measure.block<1, 3>(ct, 0) = f_tq_sens->torque();\n\t ft_forces_measure.block<1, 3>(ct, 0) = f_tq_sens->force();\n\t ft_wrench_measure.block<1, 6>(ct, 0) = f_tq_sens->wrench();\n\t ct++;\n\t}\n\t```', 'IMU_MEASUREMENT': '\t```cpp\n\tEigen::MatrixXd imu_angular_positions_measure(robot->num_bodies(), 3);\n\tEigen::MatrixXd imu_angular_velocities_measure(robot->num_bodies(), 3);\n\tEigen::MatrixXd imu_linear_acceleration_measure(robot->num_bodies(), 3);\n\tct = 0;\n\tfor (const auto& imu_sens : imu_sensors) {\n\t imu_angular_positions_measure.block<1, 3>(ct, 0) = imu_sens->angular_position_vec();\n\t imu_angular_velocities_measure.block<1, 3>(ct, 0) = imu_sens->angular_velocity();\n\t imu_linear_acceleration_measure.block<1, 3>(ct, 0) = imu_sens->linear_acceleration();\n\t ct++;\n\t}\n\t```', 'RGB_SENSOR': '\t```cpp\n\t// a nested std::vector (w*h*3) of the last image taken can be retrieved\n\tauto rgb_image = camera->image();\n\t```', 'RGB_SENSOR_MEASURE': '\t```cpp\n\t// we can also save them to png\n\trobot_dart::gui::save_png_image("camera-small.png", rgb_image);\n\t// convert an rgb image to grayscale (useful in some cases)\n\tauto gray_image = robot_dart::gui::convert_rgb_to_grayscale(rgb_image);\n\trobot_dart::gui::save_png_image("camera-gray.png", gray_image);\n\t```', 'RGB_D_SENSOR': '\t```cpp\n\t// get the depth image from a camera\n\t// with a version for visualization or bigger differences in the output\n\tauto rgb_d_image = camera->depth_image();\n\t// and the raw values that can be used along with the camera parameters to transform the image to point-cloud\n\tauto rgb_d_image_raw = camera->raw_depth_image();\n\t```', 'RGB_D_SENSOR_MEASURE': '\t```cpp\n\trobot_dart::gui::save_png_image("camera-depth.png", rgb_d_image);\n\trobot_dart::gui::save_png_image("camera-depth-raw.png", rgb_d_image_raw);\n\t```', 'SET_ACTUATOR': '\t```cpp\n\t// Set all DoFs to same actuator\n\trobot->set_actuator_types("servo"); // actuator types can be "servo", "torque", "velocity", "passive", "locked", "mimic"\n\t// You can also set actuator types separately\n\trobot->set_actuator_type("torque", "iiwa_joint_5");\n\t```', 'POSITIONS_ENFORCED': '\t```cpp\n\t// Εnforce joint position and velocity limits\n\trobot->set_position_enforced(true);\n\t```', 'MODIFY_LIMITS': '\t```cpp\n\t// Modify Position Limits\n\tEigen::VectorXd pos_upper_lims(7);\n\tpos_upper_lims << 2.096, 2.096, 2.096, 2.096, 2.096, 2.096, 2.096;\n\trobot->set_position_upper_limits(pos_upper_lims, dof_names);\n\trobot->set_position_lower_limits(-pos_upper_lims, dof_names);\n\t\n\t// Modify Velocity Limits\n\t\n\tEigen::VectorXd vel_upper_lims(7);\n\tvel_upper_lims << 1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5;\n\trobot->set_velocity_upper_limits(vel_upper_lims, dof_names);\n\trobot->set_velocity_lower_limits(-vel_upper_lims, dof_names);\n\t\n\t// Modify Force Limits\n\t\n\tEigen::VectorXd force_upper_lims(7);\n\tforce_upper_lims << 150, 150, 150, 150, 150, 150, 150;\n\trobot->set_force_upper_limits(force_upper_lims, dof_names);\n\trobot->set_force_lower_limits(-force_upper_lims, dof_names);\n\t\n\t// Modify Acceleration Limits\n\tEigen::VectorXd acc_upper_lims(7);\n\tacc_upper_lims << 1500, 1500, 1500, 1500, 1500, 1500, 1500;\n\trobot->set_acceleration_upper_limits(acc_upper_lims, dof_names);\n\trobot->set_acceleration_lower_limits(-acc_upper_lims, dof_names);\n\t```', 'MODIFY_COEFFS': '\t```cpp\n\t// Modify Damping Coefficients\n\tstd::vector damps = {0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4};\n\trobot->set_damping_coeffs(damps, dof_names);\n\t\n\t// Modify Coulomb Frictions\n\tstd::vector cfrictions = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001};\n\trobot->set_coulomb_coeffs(cfrictions, dof_names);\n\t\n\t// Modify Spring Stiffness\n\tstd::vector stiffnesses = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001};\n\trobot->set_spring_stiffnesses(stiffnesses, dof_names);\n\t```', 'SET_COLLISION_DETECTOR': '\t```cpp\n\tsimu.set_collision_detector("fcl"); // collision_detector can be "dart", "fcl", "ode" or "bullet" (case does not matter)\n\t```', 'SELF_COLLISIONS': '\t```cpp\n\tif (!robot->self_colliding()) {\n\t std::cout << "Self collision is not enabled" << std::endl;\n\t // set self collisions to true and adjacent collisions to false\n\t robot->set_self_collision(true, false);\n\t}\n\t```', 'HEXAPOD': '\t```cpp\n\tauto robot = std::make_shared();\n\t```', 'HEXAPOD_PYTHON': '\t```python\n\trobot = rd.Hexapod()\n\t```', 'SET_COLLISION_DETECTOR_PYTHON': '\t```python\n\tsimu.set_collision_detector("fcl") # collision_detector can be "dart", "fcl", "ode" or "bullet" (case does not matter)\n\t```', 'SELF_COLLISIONS_PYTHON': '\t```python\n\tif(not robot.self_colliding()):\n\t print("Self collision is not enabled")\n\t # set self collisions to true and adjacent collisions to false\n\t robot.set_self_collision(True, False)\n\t```', 'FRANKA_PYTHON': '\t```python\n\trobot = rd.Franka()\n\t```', 'PD_CONTROL_PYTHON': '\t```python\n\t# add a PD-controller to the arm\n\t# set desired positions\n\tctrl = [0., np.pi / 4., 0., -np.pi / 4., 0., np.pi / 2., 0., 0.]\n\t\n\t# add the controller to the robot\n\tcontroller = rd.PDControl(ctrl)\n\trobot.add_controller(controller)\n\tcontroller.set_pd(300., 50.)\n\t```', 'SIMPLE_ARM_PYTHON': '\t```python\n\trobot = rd.Arm()\n\t```', 'TALOS_PYTHON': '\t```python\n\trobot = rd.Talos()\n\t```', 'RECORD_VIDEO_ROBOT_GRAPHICS_PARAMS_PYTHON': '\t```python\n\tgraphics.record_video("talos_fast.mp4")\n\t```', 'CAMERAS_PARALLEL_PYTHON': '\t```python\n\trobot = rd.Robot("arm.urdf", "arm", False)\n\trobot.fix_to_world()\n\t\n\tdef test():\n\t pid = os.getpid()\n\t ii = pid%15\n\t\n\t # create the controller\n\t pdcontrol = rd.PDControl([0.0, 1.0, -1.5, 1.0], False)\n\t\n\t # clone the robot\n\t grobot = robot.clone()\n\t\n\t # add the controller to the robot\n\t grobot.add_controller(pdcontrol, 1.)\n\t pdcontrol.set_pd(200., 20.)\n\t\n\t # create the simulation object\n\t simu = rd.RobotDARTSimu(0.001)\n\t\n\t # set the graphics\n\t graphics = rd.gui.WindowlessGraphics()\n\t simu.set_graphics(graphics)\n\t\n\t graphics.look_at([0.4 * ii, 3.5 - ii * 0.1, 2.], [0., 0., 0.25], [0., 0., 1.])\n\t\n\t # add the robot and the floor\n\t simu.add_robot(grobot)\n\t simu.add_checkerboard_floor()\n\t\n\t # run\n\t simu.run(20.)\n\t\n\t # save last frame for visualization purposes\n\t img = graphics.image()\n\t rd.gui.save_png_image(\'camera-\' + str(ii) + \'.png\', img)\n\t\n\t# helper function to run in parallel\n\tdef runInParallel(N):\n\t proc = []\n\t for i in range(N):\n\t # rd.gui.run_with_gl_context accepts 2 parameters:\n\t # (func, wait_time_in_ms)\n\t # the func needs to be of the following format: void(), aka no return, no arguments\n\t p = Process(target=rd.gui.run_with_gl_context, args=(test, 20))\n\t p.start()\n\t proc.append(p)\n\t for p in proc:\n\t p.join()\n\t\n\tprint(\'Running parallel evaluations\')\n\tN = 15\n\tstart = timer()\n\trunInParallel(N)\n\tend = timer()\n\tprint(\'Time:\', end-start)\n\t```', 'HELLO_WORLD_INCLUDE_PYTHON': '\t```python\n\timport RobotDART as rd\n\t```', 'HELLO_WORLD_ROBOT_CREATION_PYTHON': '\t```python\n\trobot = rd.Robot("pexod.urdf");\n\t```', 'HELLO_WORLD_ROBOT_PLACING_PYTHON': '\t```python\n\trobot.set_base_pose([0., 0., 0., 0., 0., 0.2])\n\t```', 'HELLO_WORLD_ROBOT_SIMU_PYTHON': '\t```python\n\tsimu = rd.RobotDARTSimu(0.001); # dt=0.001, 1KHz simulation\n\tsimu.add_floor();\n\tsimu.add_robot(robot);\n\t```', 'HELLO_WORLD_ROBOT_GRAPHIC_PYTHON': '\t```python\n\tgraphics = rd.gui.Graphics()\n\tsimu.set_graphics(graphics)\n\tgraphics.look_at([0.5, 3., 0.75], [0.5, 0., 0.2])\n\t```', 'HELLO_WORLD_ROBOT_RUN_PYTHON': '\t```python\n\tsimu.run(10.)\n\t```', 'SET_ACTUATOR_PYTHON': '\t```python\n\t# Set all DoFs to same actuator\n\t# actuator types can be "servo", "torque", "velocity", "passive", "locked", "mimic"\n\trobot.set_actuator_types("servo")\n\t# You can also set actuator types separately\n\trobot.set_actuator_type("torque", "iiwa_joint_5")\n\t```', 'POSITIONS_ENFORCED_PYTHON': '\t```python\n\t# Εnforce joint position and velocity limits\n\trobot.set_position_enforced(True)\n\t```', 'MODIFY_LIMITS_PYTHON': '\t```python\n\t# Modify Position Limits\n\tpos_upper_lims = np.array([2.096, 2.096, 2.096, 2.096, 2.096, 2.096, 2.096])\n\trobot.set_position_upper_limits(pos_upper_lims, dof_names)\n\trobot.set_position_lower_limits(-1*pos_upper_lims, dof_names)\n\t\n\t# Modify Velocity Limits\n\tvel_upper_lims = np.array([1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5])\n\t\n\trobot.set_velocity_upper_limits(vel_upper_lims, dof_names)\n\trobot.set_velocity_lower_limits(-1*vel_upper_lims, dof_names)\n\t\n\t# Modify Force Limits\n\tforce_upper_lims = np.array([150, 150, 150, 150, 150, 150, 150])\n\trobot.set_force_upper_limits(force_upper_lims, dof_names)\n\trobot.set_force_lower_limits(-1*force_upper_lims, dof_names)\n\t\n\t# Modify Acceleration Limits\n\tacc_upper_lims = np.array([1500, 1500, 1500, 1500, 1500, 1500, 1500])\n\trobot.set_acceleration_upper_limits(acc_upper_lims, dof_names)\n\trobot.set_acceleration_lower_limits(-1*acc_upper_lims, dof_names)\n\t```', 'MODIFY_COEFFS_PYTHON': '\t```python\n\t# Modify Damping Coefficients\n\tdamps = [0.4, 0.4, 0.4, 0.4, 0.4, 0.4, 0.4]\n\trobot.set_damping_coeffs(damps, dof_names)\n\t\n\t# Modify Coulomb Frictions\n\tcfrictions = [0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001]\n\trobot.set_coulomb_coeffs(cfrictions, dof_names)\n\t\n\t# Modify Spring Stiffness\n\tstiffnesses = [0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001]\n\trobot.set_spring_stiffnesses(stiffnesses, dof_names)\n\t```', 'CAMERA_SENSOR_RGBD_RECORD_DEPTH_PYTHON': '\t```python\n\t# cameras are recording color images by default, enable depth images as well for this example\n\tcamera.camera().record(True, True)\n\t```', 'TORQUE_SENSOR_PYTHON': '\t```python\n\t# Add torque sensors to the robot\n\ttq_sensors = np.empty(robot.num_dofs(), dtype=rd.sensor.Torque)\n\tct = 0\n\tfor joint in robot.dof_names():\n\t simu.add_sensor(rd.sensor.Torque(robot, joint, 1000))\n\t tq_sensors[ct] = simu.sensors()[-1]\n\t ct += 1\n\t```', 'FORCE_TORQUE_SENSOR_PYTHON': '\t```python\n\t# Add force-torque sensors to the robot\n\tf_tq_sensors = np.empty(robot.num_dofs(), dtype=rd.sensor.ForceTorque)\n\tct = 0\n\tfor joint in robot.dof_names():\n\t simu.add_sensor(rd.sensor.ForceTorque(\n\t robot, joint, 1000, "parent_to_child"))\n\t f_tq_sensors[ct] = simu.sensors()[-1]\n\t print(f_tq_sensors)\n\t ct += 1\n\t```', 'IMU_SENSOR_PYTHON': '\t```python\n\t# Add IMU sensors to the robot\n\tct = 0\n\timu_sensors = np.empty(robot.num_bodies(), dtype=rd.sensor.IMU)\n\tfor body_node in robot.body_names():\n\t simu.add_sensor(rd.sensor.IMU(\n\t rd.sensor.IMUConfig(robot.body_node(body_node), 1000)))\n\t imu_sensors[ct] = simu.sensors()[-1]\n\t ct += 1\n\t```', 'TORQUE_MEASUREMENT_PYTHON': '\t```python\n\t# vector that contains the torque measurement for every joint (scalar)\n\ttorques_measure = np.empty(robot.num_dofs())\n\tct = 0\n\tfor tq_sens in tq_sensors:\n\t torques_measure[ct] = tq_sens.torques()\n\t ct += 1\n\t```', 'FORCE_TORQUE_MEASUREMENT_PYTHON': '\t```python\n\t# matrix that contains the torque measurement for every joint (3d vector)\n\tft_torques_measure = np.empty([robot.num_dofs(), 3])\n\t# matrix that contains the force measurement for every joint (3d vector)\n\tft_forces_measure = np.empty([robot.num_dofs(), 3])\n\t# matrix that contains the wrench measurement for every joint (6d vector)[torque, force]\n\tft_wrench_measure = np.empty([robot.num_dofs(), 6])\n\tct = 0\n\tfor f_tq_sens in f_tq_sensors:\n\t ft_torques_measure[ct, :] = f_tq_sens.torque()\n\t ft_forces_measure[ct, :] = f_tq_sens.force()\n\t ft_wrench_measure[ct, :] = f_tq_sens.wrench()\n\t ct += 1\n\t\n\t```', 'IMU_MEASUREMENT_PYTHON': '\t```python\n\timu_angular_positions_measure = np.empty([robot.num_bodies(), 3])\n\timu_angular_velocities_measure = np.empty([robot.num_bodies(), 3])\n\timu_linear_acceleration_measure = np.empty([robot.num_bodies(), 3])\n\tct = 0\n\tfor imu_sens in imu_sensors:\n\t imu_angular_positions_measure[ct,:] = imu_sens.angular_position_vec()\n\t imu_angular_velocities_measure[ct, :] = imu_sens.angular_velocity()\n\t imu_linear_acceleration_measure[ct,:] = imu_sens.linear_acceleration()\n\t ct += 1\n\t\n\t```', 'RGB_SENSOR_PYTHON': '\t```python\n\t# a nested array (w*h*3) of the last image taken can be retrieved\n\trgb_image = camera.image()\n\t```', 'RGB_SENSOR_MEASURE_PYTHON': '\t```python\n\t# we can also save them to png\n\trd.gui.save_png_image("camera-small.png", rgb_image)\n\t# convert an rgb image to grayscale (useful in some cases)\n\tgray_image = rd.gui.convert_rgb_to_grayscale(rgb_image)\n\trd.gui.save_png_image("camera-gray.png", gray_image)\n\t```', 'RGB_D_SENSOR_PYTHON': '\t```python\n\t# get the depth image from a camera\n\t# with a version for visualization or bigger differences in the output\n\trgb_d_image = camera.depth_image()\n\t# and the raw values that can be used along with the camera parameters to transform the image to point-cloud\n\trgb_d_image_raw = camera.raw_depth_image()\n\t```', 'RGB_D_SENSOR_MEASURE_PYTHON': '\t```python\n\trd.gui.save_png_image("camera-depth.png", rgb_d_image)\n\trd.gui.save_png_image("camera-depth-raw.png", rgb_d_image_raw)\n\t```', 'A1_PYTHON': '\t```python\n\trobot = rd.A1()\n\t```', 'A1_PRINT_IMU_PYTHON': '\t```python\n\tprint( "Angular Position: ", robot.imu().angular_position_vec().transpose())\n\tprint( "Angular Velocity: ", robot.imu().angular_velocity().transpose())\n\tprint( "Linear Acceleration: ", robot.imu().linear_acceleration().transpose())\n\tprint( "=================================")\n\t```', 'SIMPLE_CONTROL_PYTHON': '\t```python\n\tcontroller1 = rd.SimpleControl(ctrl)\n\t# add the controller to the robot, with a default weight of 1.0\n\trobot.add_controller(controller1)\n\t```', 'INIT_SIMU_PYTHON': '\t```python\n\t# choose time step of 0.001 seconds\n\tsimu = rd.RobotDARTSimu(0.001)\n\t```', 'MODIFY_SIMU_DT_PYTHON': '\t```python\n\t# set timestep to 0.005 and update control frequency(bool)\n\tsimu.set_timestep(0.005, True)\n\t```', 'SIMU_GRAVITY_PYTHON': '\t```python\n\t# set gravitational force of mars\n\tmars_gravity = [0., 0., -3.721]\n\tsimu.set_gravity(mars_gravity)\n\t```', 'GRAPHICS_PARAMS_PYTHON': '\t```python\n\tconfiguration = rd.gui.GraphicsConfiguration()\n\t# We can change the width/height of the window (or camera, dimensions)\n\tconfiguration.width = 1280\n\tconfiguration.height = 960\n\tconfiguration.title = "Graphics Tutorial" # We can set a title for our window\n\t\n\t# We can change the configuration for shadows\n\tconfiguration.shadowed = True\n\tconfiguration.transparent_shadows = True\n\tconfiguration.shadow_map_size = 1024\n\t\n\t# We can also alter some specifications for the lighting\n\tconfiguration.max_lights = 3 # maximum number of lights for our scene\n\tconfiguration.specular_strength = 0.25 # strength og the specular component\n\t\n\t# Some extra configuration for the main camera\n\tconfiguration.draw_main_camera = True\n\tconfiguration.draw_debug = True\n\tconfiguration.draw_text = True\n\t\n\t# We can also change the background color [default = black]\n\tconfiguration.bg_color = [1., 1., 1., 1.]\n\t\n\t# create the graphics object with the configuration as a parameter\n\tgraphics = rd.gui.Graphics(configuration)\n\t```', 'SHADOWS_GRAPHICS_PYTHON': '\t```python\n\t# Disable shadows\n\tgraphics.enable_shadows(False, False)\n\tsimu.run(1.)\n\t# Enable shadows only for non-transparent objects\n\tgraphics.enable_shadows(True, False)\n\tsimu.run(1.)\n\t# Enable shadows for transparent objects as well\n\tgraphics.enable_shadows(True, True)\n\tsimu.run(1.)\n\t```', 'CLR_LIGHT_PYTHON': '\t```python\n\t# Clear Lights\n\tgraphics.clear_lights()\n\t```', 'LIGHT_MATERIAL_PYTHON': '\t```python\n\t# Clear Light material\n\tshininess = 1000.\n\twhite = magnum.Color4(1., 1., 1., 1.)\n\t\n\t# ambient, diffuse, specular\n\tcustom_material = rd.gui.Material(white, white, white, shininess)\n\t```', 'POINT_LIGHT_PYTHON': '\t```python\n\t# Create point light\n\tposition = magnum.Vector3(0., 0., 2.)\n\tintensity = 1.\n\tattenuation_terms = magnum.Vector3(1., 0., 0.)\n\tpoint_light = rd.gui.create_point_light(position, custom_material, intensity, attenuation_terms)\n\tgraphics.add_light(point_light)\n\t```', 'DIRECTIONAL_LIGHT_PYTHON': '\t```python\n\t# Create directional light\n\tdirection = magnum.Vector3(-1, -1, -1)\n\tdirectional_light = rd.gui.create_directional_light(direction, custom_material)\n\tgraphics.add_light(directional_light)\n\t```', 'SPOT_LIGHT_PYTHON': '\t```python\n\t# Create spot light\n\tposition = magnum.Vector3(0., 0., 1.)\n\tdirection = magnum.Vector3(-1, -1, -1)\n\tintensity = 1.\n\tattenuation_terms = magnum.Vector3(1., 0., 0.)\n\tspot_exponent = np.pi\n\tspot_cut_off = np.pi / 8\n\tspot_light = rd.gui.create_spot_light(position, custom_material, direction, spot_exponent, spot_cut_off, intensity, attenuation_terms)\n\tgraphics.add_light(spot_light)\n\t```', 'LOAD_IICUB_PYTHON': '\t```python\n\trobot = rd.ICub()\n\t\n\t# Set actuator types to VELOCITY motors so that they stay in position without any controller\n\trobot.set_actuator_types("velocity")\n\tsimu = rd.RobotDARTSimu(0.001)\n\tsimu.set_collision_detector("fcl")\n\t```', 'ICUB_PRINT_IMU_PYTHON': '\t```python\n\tprint("Angular Position: ", robot.imu().angular_position_vec().transpose())\n\tprint("Angular Velocity: ", robot.imu().angular_velocity().transpose())\n\tprint("Linear Acceleration: ", robot.imu().linear_acceleration().transpose())\n\tprint("=================================" )\n\t```', 'ICUB_PRINT_FT_PYTHON': '\t```python\n\tprint("FT ( force): ", robot.ft_foot_left().force().transpose())\n\tprint("FT (torque): ", robot.ft_foot_left().torque().transpose())\n\tprint("=================================")\n\t```', 'ADD_NEW_CAMERA_PYTHON': '\t```python\n\t# Add camera\n\tcamera = rd.sensor.Camera(graphics.magnum_app(), 32, 32)\n\t```', 'MANIPULATE_CAM_SEP_PYTHON': '\t```python\n\tcamera.camera().set_far_plane(5.)\n\tcamera.camera().set_near_plane(0.01)\n\tcamera.camera().set_fov(60.0)\n\t```', 'MANIPULATE_CAM_PYTHON': '\t```python\n\tcamera.camera().set_camera_params(5., #far plane\n\t 0.01, #near plane\n\t 60.0, # field of view\n\t 600, # width\n\t 400) #height\n\t```', 'RECORD_VIDEO_CAMERA_PYTHON': '\t```python\n\t\n\t# cameras can also record video\n\tcamera.record_video("video-camera.mp4")\n\t```', 'CAM_POSITION_PYTHON': '\t```python\n\t# set the position of the camera, and the position where the main camera is looking at\n\tcam_pos = [-0.5, -3., 0.75]\n\tcam_looks_at = [0.5, 0., 0.2]\n\tcamera.look_at(cam_pos, cam_looks_at)\n\t```', 'CAM_ATTACH_PYTHON': '\t```python\n\ttf = dartpy.math.Isometry3()\n\trot = dartpy.math.AngleAxis(3.14, [1., 0., 0.])\n\trot = rot.multiply( dartpy.math.AngleAxis(1.57, [0., 0., 1.])).to_rotation_matrix()\n\ttf.set_translation([0., 0., 0.1])\n\tcamera.attach_to_body(robot.body_node("iiwa_link_ee"), tf) # cameras are looking towards -z by default\n\t```', 'TALOS_FAST_PYTHON': '\t```python\n\trobot = rd.TalosFastCollision()\n\t```', 'KINEMATICS_PYTHON': '\t```python\n\t# Get Joint Positions(Angles)\n\tjoint_positions = robot.positions()\n\t\n\t# Get Joint Velocities\n\tjoint_vels = robot.velocities()\n\t\n\t# Get Joint Accelerations\n\tjoint_accs = robot.accelerations()\n\t\n\t# Get link_name(str) Transformation matrix with respect to the world frame.\n\teef_tf = robot.body_pose(link_name)\n\t\n\t# Get translation vector from transformation matrix\n\teef_pos = eef_tf.translation()\n\t\n\t# Get rotation matrix from tranformation matrix\n\teef_rot = eef_tf.rotation()\n\t\n\t# Get link_name 6d pose vector [logmap(eef_tf.linear()), eef_tf.translation()]\n\teef_pose_vec = robot.body_pose_vec(link_name)\n\t\n\t# Get link_name 6d velocity vector [angular, cartesian]\n\teef_vel = robot.body_velocity(link_name)\n\t\n\t# Get link_name 6d acceleration vector [angular, cartesian]\n\teef_acc = robot.body_acceleration(link_name)\n\t\n\t# Jacobian targeting the origin of link_name(str)\n\tjacobian = robot.jacobian(link_name)\n\t\n\t# Jacobian time derivative\n\tjacobian_deriv = robot.jacobian_deriv(link_name)\n\t\n\t# Center of Mass Jacobian\n\tcom_jacobian = robot.com_jacobian()\n\t\n\t# Center of Mass Jacobian Time Derivative\n\tcom_jacobian_deriv = robot.com_jacobian_deriv()\n\t```', 'DYNAMICS_PYTHON': "\t```python\n\t# Get Joint Forces\n\tjoint_forces = robot.forces()\n\t\n\t# Get link's mass\n\teef_mass = robot.body_mass(link_name)\n\t\n\t# Mass Matrix of robot\n\tmass_matrix = robot.mass_matrix()\n\t\n\t# Inverse of Mass Matrix\n\tinv_mass_matrix = robot.inv_mass_matrix()\n\t\n\t# Augmented Mass matrix\n\taug_mass_matrix = robot.aug_mass_matrix()\n\t\n\t# Inverse of Augmented Mass matrix\n\tinv_aug_mass_matrix = robot.inv_aug_mass_matrix()\n\t\n\t# Coriolis Force vector\n\tcoriolis = robot.coriolis_forces()\n\t\n\t# Gravity Force vector\n\tgravity = robot.gravity_forces()\n\t\n\t# Combined vector of Coriolis Force and Gravity Force\n\tcoriolis_gravity = robot.coriolis_gravity_forces()\n\t\n\t# NOT IMPLEMENTED\n\t# # Constraint Force Vector\n\t# constraint_forces = robot.constraint_forces()\n\t\n\t```", 'ROBOT_CONTROL_PYTHON': '\t```python\n\tclass MyController(rd.RobotControl):\n\t def __init__(self, ctrl, full_control):\n\t rd.RobotControl.__init__(self, ctrl, full_control)\n\t\n\t def __init__(self, ctrl, controllable_dofs):\n\t rd.RobotControl.__init__(self, ctrl, controllable_dofs)\n\t\n\t def configure(self):\n\t self._active = True\n\t\n\t def calculate(self, t):\n\t target = np.array([self._ctrl])\n\t cmd = 100*(target-self.robot().positions(self._controllable_dofs))\n\t return cmd[0]\n\t\n\t # TO-DO: This is NOT working at the moment!\n\t def clone(self):\n\t return MyController(self._ctrl, self._controllable_dofs)\n\t```', 'IIWA_PYTHON': '\t```python\n\trobot = rd.Iiwa()\n\t```', 'ROBOT_GHOST_PYTHON': '\t```python\n\t# Add a ghost robot; only visuals, no dynamics, no collision\n\tghost = robot.clone_ghost()\n\tsimu.add_robot(ghost)\n\t```'} for v in variables.items(): env.variables[v[0]] = variables[v[0]] diff --git a/src/examples/pendulum.cpp b/src/examples/pendulum.cpp index 04fa19dc..508a7c67 100644 --- a/src/examples/pendulum.cpp +++ b/src/examples/pendulum.cpp @@ -14,18 +14,21 @@ int main() ctrl << 0.0; // @SIMPLE_CONTROL@ auto controller1 = std::make_shared(ctrl); + // add the controller to the robot, with a default weight of 1.0 robot->add_controller(controller1); // @SIMPLE_CONTROL_END@ ctrl << -1.0; auto controller2 = std::make_shared(ctrl); + // add the controller to the robot, with a weight of 5.0 robot->add_controller(controller2, 5.); + // initialize the simulation with a default timestep of 0.015s robot_dart::RobotDARTSimu simu; #ifdef GRAPHIC simu.set_graphics(std::make_shared()); #endif simu.add_robot(robot); - + // the the dimensions of the pendulum to calculate the end effector position Eigen::Vector3d size(0.0402, 0.05, 1); std::cout << (robot->body_pose("pendulum_link_1") * size).transpose() << std::endl; simu.run(2.5); @@ -34,7 +37,7 @@ int main() controller1->set_parameters(ctrl); simu.run(2.5); std::cout << (robot->body_pose("pendulum_link_1") * size).transpose() << std::endl; - + // reset the positions, velocities, and accelerations of the robot, clear internal and external forces, and also commands robot.reset(); return 0; } diff --git a/src/examples/python/pendulum.py b/src/examples/python/pendulum.py index 8b01ab60..ae4ff1de 100644 --- a/src/examples/python/pendulum.py +++ b/src/examples/python/pendulum.py @@ -10,16 +10,20 @@ ctrl = [0.0] # @SIMPLE_CONTROL_PYTHON@ controller1 = rd.SimpleControl(ctrl) +# add the controller to the robot, with a default weight of 1.0 robot.add_controller(controller1) # @SIMPLE_CONTROL_PYTHON_END@ ctrl = [-1.0] controller2 = rd.SimpleControl(ctrl) +# add the controller to the robot, with a weight of 5.0 robot.add_controller(controller2, 5.) +# initialize the simulation with a default timestep of 0.015s simu = rd.RobotDARTSimu() simu.set_graphics(rd.gui.Graphics()) simu.add_robot(robot) +# the dimensions of the pendulum to calculate the end effector position size = [0.0402, 0.05, 1] print((robot.body_pose("pendulum_link_1").multiply(size)).transpose()) simu.run(2.5) @@ -29,4 +33,5 @@ simu.run(2.5) print((robot.body_pose("pendulum_link_1").multiply(size)).transpose()) +# reset the positions, velocities, and accelerations of the robot, clear internal and external forces, and also commands robot.reset()