Rayrai Example: Inverse Kinematics
Overview
A Kinova arm with a Robotiq 2F-85 gripper reaches for a target that you move with the mouse. Every frame, ArticulatedSystem::solveIK() computes the joint angles that put the gripper's grasp frame on the target, in position and, if you choose, in orientation.
The arm is simulated, not posed. Its joint PD controllers drive it towards the IK solution at a limited joint speed, so it pushes the cubes on the workbench aside, and with the gripper closed it can pick them up.
The example renders in-process with raisin::RayraiWindow; it needs no TCP viewer.
Target
CMake target: rayrai_inverse_kinematics.
Run
Run the build-tree executable:
bash
./build-examples/examples/rayrai_inverse_kinematicsOn Windows, run rayrai_inverse_kinematics.exe instead. The example takes no arguments. --tour moves the target along the figure eight of the clip above until you grab it, and --frames=N exits after N frames.
Controls
| Input | Effect |
|---|---|
| Left-drag the orange ball | Move the target in the plane that faces the camera |
| Mouse wheel over the ball | Push the target away from the camera or pull it closer |
| Right-drag | Turn the target, while Follow the target's orientation is on |
| Left-drag elsewhere, mouse wheel | Orbit and zoom the camera |
G | Close or open the gripper |
R | Reset the arm, the cubes and the target |
The panel switches the orientation task on and off and selects the IK method: damped least squares (with its damping), pseudo-inverse, SVD or Jacobian transpose. It shows whether the last solve converged and how far the gripper is from the target. The ball turns red when the solver cannot reach the target.
How it works
The scene and the controller are in include/arm_ik_demo.hpp. Each frame the example does three things:
The mouse moves the target. The ray through the pointer is intersected with the plane through the target that faces the camera. The grabbed point stays under the pointer, so the ball does not jump to the cursor when you grab it off-centre.
IK solves for the target.
Scene::SolveIk()starts from the previous solution and leaves the simulated arm alone:cppik_options_.seed.assign(ik_solution_.data(), ik_solution_.data() + ik_solution_.size()); ik_options_.applySolution = false; ik_options_.useOrientation = track_orientation_; ik_result_ = robot_->solveIK(ik_frame_, position, orientation, ik_options_); const auto& solution = ik_result_.success ? ik_result_.solution : ik_result_.bestSolution;- Seed. Starting from the previous solution keeps the arm on one IK branch. The joint angles change continuously as the target moves; a fresh solve from an arbitrary pose could flip the elbow or the wrist.
applySolution = false. The solver restores the arm's state when it returns, so only the physics moves the arm.bestSolution. Out of reach, it is the closest pose the solver found, and the arm stretches towards the target.orientationWeight = 0.3. When the target cannot be reached in both position and orientation, the solver trades 1 rad of hand orientation for 0.3 m of position (seeIKOptions).
The arm follows.
Scene::Step()moves the joint PD targets towards the solution with a time constant of 0.08 s. All joints are scaled together so that none turns faster than 2 rad/s, and they arrive at the same time. The velocity targets are the rates of those position targets. Then the world integrates one 1 ms step. The gripper's knuckle ramps the same way; its five mimic joints follow passively (see Mimic Joints).
What to look for
- Singular poses. With the gripper pointing down, the arm nears singular configurations when the target is high above the bench or far out. There, small turns of the hand need large joint motions or cannot be made at all: tilt the target and the ball turns red. The home pose is low over the bench, away from them.
- Physical tracking. While the target moves, the gripper trails it by about the approach time constant. Once the target stops, the gripper settles within a few millimetres; the panel shows the distance.
- Grasping. Lower the target onto a cube with the fingers square to it, press
G, and lift: friction at the finger pads holds the 100 g cube. - Jacobian transpose. It takes many small steps. With 50 iterations per frame it needs about a second and a half to converge on a target 17 cm away, which the other methods reach in the first frame.
Full source
cpp
// Inverse kinematics with the mouse: a Kinova arm with a Robotiq gripper
// reaches for a target that you move around its workbench.
//
// Every frame, ArticulatedSystem::solveIK() finds the joint angles that put the
// gripper's grasp frame on the target, starting from the previous solution.
// The arm itself is simulated: its joint PD targets follow that solution at a
// limited speed, so it pushes the cubes it touches and can pick them up
// (include/arm_ik_demo.hpp).
//
// Left-drag the orange ball move the target in the plane facing the camera
// Wheel over the ball move the target away from or towards the camera
// Right-drag turn the target (while orientation is followed)
// Left-drag elsewhere, wheel orbit and zoom the camera
// G / R close or open the gripper / reset the scene
//
// --tour moves the target along a scripted path and --frames=N exits after N
// frames; they exist for the documentation's clip and the tests.
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <memory>
#include <string>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/quaternion.hpp>
#include "arm_ik_demo.hpp"
#include "example_resources.hpp"
#include "rayrai/PhysicsSnapshot.hpp"
#include "rayrai/example_common.hpp"
#include "rayrai_example_compat.hpp"
namespace {
using raisim_examples::arm_ik::Ray;
using raisim_examples::arm_ik::Scene;
constexpr double kHandleRadius = 0.025;
// The handle can be grabbed this many pixels outside its outline.
constexpr double kGrabSlackPixels = 10.;
// Target travel per mouse-wheel notch [m].
constexpr double kWheelStep = 0.02;
constexpr double kRadiansPerPixel = 0.01;
constexpr const char* kMethods[] = {"dls", "pinv", "svd", "transpose"};
Eigen::Vector3d toEigen(const glm::vec3& v) { return {v.x, v.y, v.z}; }
glm::quat toGlm(const Eigen::Matrix3d& rotation) {
const Eigen::Quaterniond q(rotation);
return {float(q.w()), float(q.x()), float(q.y()), float(q.z())};
}
/// The mouse pointer over the full-window viewer image.
struct Pointer {
bool hovered = false; ///< Over the image and not over a panel.
double x = 0., y = 0.; ///< Drawable pixels, y down.
int width = 0, height = 0;
};
/// World-space ray through a pixel of the viewer image.
Ray pointerRay(const raisin::Camera& camera, const Pointer& pointer) {
const glm::vec4 viewport(0.f, 0.f, float(pointer.width), float(pointer.height));
const glm::mat4 view = camera.getViewMatrix(), projection = camera.getProjectionMatrix();
// OpenGL window coordinates grow upwards.
const glm::vec2 pixel(float(pointer.x), float(pointer.height - pointer.y));
const glm::vec3 nearPoint = glm::unProject(glm::vec3(pixel, 0.f), view, projection, viewport);
const glm::vec3 farPoint = glm::unProject(glm::vec3(pixel, 1.f), view, projection, viewport);
return {toEigen(nearPoint), toEigen(farPoint - nearPoint).normalized()};
}
/// Moves and turns the scene's target with the mouse.
class TargetControl {
public:
/// Applies this frame's mouse input to the target. Returns whether the
/// camera may use the pointer: not while the handle is hovered or held.
bool update(const Pointer& pointer, const raisin::Camera& camera, Scene& scene) {
const ImGuiIO& io = ImGui::GetIO();
touched_ = false;
if (!io.MouseDown[ImGuiMouseButton_Left]) drag_.End();
if (!io.MouseDown[ImGuiMouseButton_Right]) turning_ = false;
// Outside the window, ImGui has no pointer position to cast a ray from.
if (pointer.width <= 0 || pointer.height <= 0 || !ImGui::IsMousePosValid()) {
hovered_ = false;
return true;
}
const Ray ray = pointerRay(camera, pointer);
const Eigen::Vector3d view = toEigen(camera.front).normalized();
Eigen::Vector3d position = scene.target_position();
Eigen::Matrix3d orientation = scene.target_orientation();
// A small ball is easy to miss, so accept a few pixels around it.
const double depth = std::max(1e-3, (position - toEigen(camera.position)).dot(view));
const double metresPerPixel = 2. * depth * std::tan(glm::radians(camera.zoom) * 0.5) / pointer.height;
const double grabRadius = kHandleRadius + kGrabSlackPixels * metresPerPixel;
// While the camera orbits, the handle passing under the pointer stays inactive.
const bool orbiting = io.MouseDown[ImGuiMouseButton_Left] && !io.MouseClicked[ImGuiMouseButton_Left] && !drag_.active();
hovered_ = pointer.hovered && !orbiting && raisim_examples::arm_ik::DistanceToRay(ray, position) <= grabRadius;
if (hovered_ && io.MouseClicked[ImGuiMouseButton_Left]) drag_.Begin(ray, position, view, grabRadius);
if (pointer.hovered && io.MouseClicked[ImGuiMouseButton_Right]) turning_ = true;
const double push = drag_.active() || hovered_ ? kWheelStep * io.MouseWheel : 0.;
if (drag_.active()) {
drag_.Push(push);
position = drag_.Move(ray, position);
} else {
position += push * view;
}
const bool turn = turning_ && scene.track_orientation() && (io.MouseDelta.x != 0.f || io.MouseDelta.y != 0.f);
if (turn) {
orientation = raisim_examples::arm_ik::RotateByDrag(orientation, io.MouseDelta.x, io.MouseDelta.y, toEigen(camera.right), toEigen(camera.up),
kRadiansPerPixel);
}
touched_ = drag_.active() || turn || push != 0.;
if (touched_) scene.SetTarget(position, orientation);
return !drag_.active() && !hovered_;
}
/// The handle is under the pointer or held.
bool hot() const { return hovered_ || drag_.active(); }
/// The user moved or turned the target this frame.
bool touched() const { return touched_; }
private:
raisim_examples::arm_ik::TargetDrag drag_;
bool hovered_ = false, turning_ = false, touched_ = false;
};
/// Draws the viewer image across the window, as ExampleApp::renderViewer()
/// does, and returns the pointer over it. endViewport() renders the image.
Pointer beginViewport(ExampleApp& app, raisin::RayraiWindow& viewer) {
int fbW = 0, fbH = 0, windowW = 0, windowH = 0;
SDL_GL_GetDrawableSize(app.window, &fbW, &fbH);
SDL_GetWindowSize(app.window, &windowW, &windowH);
const int displayW = windowW > 0 ? windowW : fbW, displayH = windowH > 0 ? windowH : fbH;
ImGui::SetNextWindowPos(ImVec2(0, 0));
ImGui::SetNextWindowSize(ImVec2(float(displayW), float(displayH)));
ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(0.f, 0.f));
ImGui::Begin("Viewer", nullptr,
ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoSavedSettings |
ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoScrollWithMouse | ImGuiWindowFlags_NoBringToFrontOnFocus);
const ImVec2 origin = ImGui::GetCursorScreenPos();
ImGui::Image((ImTextureID)(intptr_t)viewer.getImageTexture(), ImVec2(float(displayW), float(displayH)), ImVec2(0, 1), ImVec2(1, 0));
// Window points to drawable pixels (they differ on high-DPI displays).
const ImVec2 mouse = ImGui::GetIO().MousePos;
Pointer pointer;
pointer.hovered = ImGui::IsItemHovered();
pointer.x = (mouse.x - origin.x) * (displayW > 0 ? double(fbW) / displayW : 1.);
pointer.y = (mouse.y - origin.y) * (displayH > 0 ? double(fbH) / displayH : 1.);
pointer.width = fbW;
pointer.height = fbH;
return pointer;
}
void endViewport(raisin::RayraiWindow& viewer, const Pointer& pointer, bool cameraUsesPointer) {
const int x = std::clamp(int(pointer.x), 0, std::max(0, pointer.width - 1));
const int y = std::clamp(int(pointer.y), 0, std::max(0, pointer.height - 1));
// Clicks would make the camera follow the clicked body; this demo orbits the bench.
viewer.update(pointer.width, pointer.height, pointer.hovered && cameraUsesPointer, x, y, false);
ImGui::End();
ImGui::PopStyleVar();
}
void drawPanel(Scene& scene, bool& tour, double& tourTime, int& method, bool& reset) {
const auto& ik = scene.ik_result();
Eigen::Vector3d hand;
Eigen::Matrix3d handOrientation;
scene.GraspPose(hand, handOrientation);
ImGui::SetNextWindowPos(ImVec2(12, 12), ImGuiCond_FirstUseEver);
ImGui::SetNextWindowBgAlpha(0.88f);
ImGui::Begin("Inverse kinematics", nullptr, ImGuiWindowFlags_AlwaysAutoResize);
if (ik.success) {
ImGui::TextColored(ImVec4(0.45f, 0.85f, 0.45f, 1.f), "IK converged");
} else {
ImGui::TextColored(ImVec4(1.f, 0.4f, 0.35f, 1.f), "Out of reach: best pose %.0f mm, %.1f deg off", 1e3 * ik.bestPositionResidual,
glm::degrees(ik.bestOrientationResidual));
}
ImGui::Text("Gripper %.1f mm from the target", 1e3 * (hand - scene.target_position()).norm());
ImGui::Separator();
bool trackOrientation = scene.track_orientation();
if (ImGui::Checkbox("Follow the target's orientation", &trackOrientation)) scene.SetTrackOrientation(trackOrientation);
if (ImGui::Combo("Method", &method, "Damped least squares\0Pseudo-inverse\0SVD\0Jacobian transpose\0")) scene.ik_options().inverseMethod = kMethods[method];
if (method == 0) {
float damping = float(scene.ik_options().damping);
if (ImGui::SliderFloat("Damping", &damping, 0.001f, 0.2f, "%.3f", ImGuiSliderFlags_Logarithmic)) scene.ik_options().damping = damping;
}
if (ImGui::Button(scene.gripper_closed() ? "Open gripper (G)" : "Close gripper (G)")) scene.SetGripperClosed(!scene.gripper_closed());
ImGui::SameLine();
reset = ImGui::Button("Reset (R)") || reset;
ImGui::SameLine();
if (ImGui::Checkbox("Tour", &tour) && tour) tourTime = 0.;
if (ImGui::CollapsingHeader("Joint angles")) {
ImGui::TextDisabled("joint IK solution arm");
const auto& q = scene.robot()->getGeneralizedCoordinate();
for (std::size_t i = 0; i < 6; ++i) {
const std::size_t index = scene.arm_index(i);
ImGui::Text(" %zu %8.1f deg %6.1f deg", i + 1, glm::degrees(scene.ik_solution()[index]), glm::degrees(q[index]));
}
}
ImGui::Separator();
ImGui::TextDisabled("Left-drag the orange ball: move the target\n"
"Wheel over the ball: push it away or pull it closer\n"
"Right-drag: turn the target\n"
"Left-drag elsewhere / wheel: orbit / zoom");
ImGui::End();
}
} // namespace
int main(int argc, char* argv[]) {
bool tour = false;
int frames = -1;
for (int i = 1; i < argc; ++i) {
const std::string arg(argv[i]);
if (arg == "--tour") {
tour = true;
} else if (arg.rfind("--frames=", 0) == 0 && std::atoi(arg.c_str() + 9) > 0) {
frames = std::atoi(arg.c_str() + 9);
} else {
std::cerr << "usage: " << argv[0] << " [--tour] [--frames=N]\n";
return 2;
}
}
raisim::World::setActivationKey(exampleRscPath(argv[0], "activation.raisim"));
Scene scene(exampleRscPath(argv[0], "robotiq_2f85/kinova_robotiq.urdf"));
ExampleApp app;
if (!app.init("RaiSim inverse kinematics", 1280, 720)) return 77;
// Keep ImGui scratch files out of the working directory.
ImGui::GetIO().IniFilename = nullptr;
int status = 0;
{
// The renderer owns an empty world; physics reaches it only through copied snapshots.
raisin::PhysicsSnapshotCapture snapshotCapture;
raisin::RayraiWindow viewer(std::make_shared<raisim::World>(), 1280, 720);
viewer.setRenderQualitySettings(raisin::RayraiWindow::defaultRenderQualitySettings(raisin::RayraiWindow::RenderQualityPreset::Balanced));
raisim_examples::setRayraiBackgroundColorRgb255(viewer, {30, 34, 42, 255});
raisim_examples::addRayraiBasicSceneLights(viewer);
// The camera orbits the target's home, from the side of the bench where
// the arm's base shows.
auto& camera = viewer.getCamera();
const Eigen::Vector3d& home = scene.target_position();
camera.target = glm::vec3(home.x(), home.y(), home.z());
camera.position = camera.target + glm::vec3(0.5f, -1.35f, 0.8f);
const glm::vec3 direction = glm::normalize(camera.target - camera.position);
camera.yaw = glm::degrees(std::atan2(direction.y, direction.x));
camera.pitch = glm::degrees(std::asin(direction.z));
camera.zoom = 40.f;
camera.setCameraFixedTarget(true);
camera.setCameraFixedDistance(true);
camera.update(false);
auto handle = viewer.addVisualSphere("ik_target", kHandleRadius, 1.f, 0.45f, 0.05f, 1.f);
auto axes = viewer.addCoordinateFrame("ik_target_axes");
axes->frameSize = 0.1;
axes->poses.resize(1);
TargetControl control;
double tourTime = 0., accumulator = 0.;
int method = 0, frame = 0;
auto previous = std::chrono::steady_clock::now();
while (!app.quit && (frames < 0 || frame < frames)) {
app.processEvents();
if (app.quit) break;
app.beginFrame();
bool reset = false;
if (!ImGui::GetIO().WantTextInput) {
if (ImGui::IsKeyPressed(ImGuiKey_G, false)) scene.SetGripperClosed(!scene.gripper_closed());
reset = ImGui::IsKeyPressed(ImGuiKey_R, false);
}
// The mouse moves the target before the arm reaches for it.
const Pointer pointer = beginViewport(app, viewer);
const bool cameraUsesPointer = control.update(pointer, viewer.getCamera(), scene);
if (control.touched()) tour = false;
const auto now = std::chrono::steady_clock::now();
accumulator += frames >= 0 ? 1. / 60. : std::min(0.05, std::chrono::duration<double>(now - previous).count());
previous = now;
const int steps = int(accumulator / raisim_examples::arm_ik::kTimeStep);
accumulator -= steps * raisim_examples::arm_ik::kTimeStep;
if (tour) {
tourTime += steps * raisim_examples::arm_ik::kTimeStep;
Eigen::Vector3d position;
Eigen::Matrix3d orientation;
scene.TourPose(tourTime, position, orientation);
scene.SetTarget(position, orientation);
}
scene.SolveIk();
for (int i = 0; i < steps; ++i) scene.Step();
viewer.applyPhysicsSnapshot(snapshotCapture.capture(scene.world()));
const Eigen::Vector3d& target = scene.target_position();
handle->setPosition(target.x(), target.y(), target.z());
const glm::vec3 color = scene.ik_result().success ? glm::vec3(1.f, 0.45f, 0.05f) : glm::vec3(0.95f, 0.1f, 0.1f);
handle->setColor(glm::vec4(control.hot() ? glm::mix(color, glm::vec3(1.f), 0.45f) : color, 1.f));
axes->enable(scene.track_orientation());
axes->poses[0].position = glm::vec3(target.x(), target.y(), target.z());
axes->poses[0].quaternion = toGlm(scene.target_orientation());
endViewport(viewer, pointer, cameraUsesPointer);
drawPanel(scene, tour, tourTime, method, reset);
app.endFrame();
if (reset) {
scene.Reset();
tour = false;
accumulator = 0.;
}
++frame;
}
if (frames >= 0) {
Eigen::Vector3d hand;
Eigen::Matrix3d orientation;
scene.GraspPose(hand, orientation);
const Eigen::Vector3d& target = scene.target_position();
const Eigen::Quaterniond turn(scene.target_orientation());
std::cout << "frames=" << frame << " ik_converged=" << scene.ik_result().success << " gripper_to_target_mm=" << 1e3 * (hand - target).norm() << "\ntarget=" << target.x()
<< ',' << target.y() << ',' << target.z() << " target_quaternion=" << turn.w() << ',' << turn.x() << ',' << turn.y() << ',' << turn.z()
<< "\ncamera=" << camera.position.x << ',' << camera.position.y << ',' << camera.position.z << " camera_yaw=" << camera.yaw
<< " camera_pitch=" << camera.pitch << '\n';
if (gl::glGetError() != gl::GL_NO_ERROR) {
std::cerr << "OpenGL error in the inverse kinematics example\n";
status = 1;
}
}
}
app.shutdown();
return status;
}cpp
// This file is part of RaiSim. You must obtain a valid license from RaiSim Tech
// Inc. prior to usage.
#ifndef RAISIM_EXAMPLES_INCLUDE_ARM_IK_DEMO_HPP_
#define RAISIM_EXAMPLES_INCLUDE_ARM_IK_DEMO_HPP_
#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <stdexcept>
#include <string>
#include <vector>
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "raisim/World.hpp"
// The scene, the IK controller and the pointer math of rayrai_arm_ik. The arm
// is the Kinova/Robotiq model of robotiq_pick_and_place.hpp on the same
// workbench, with three loose cubes instead of the scripted one.
namespace raisim_examples::arm_ik {
constexpr double kTimeStep = 0.001;
constexpr double kTableHeight = 0.32;
constexpr double kCubeSize = 0.04;
// Knuckle angle that closes the fingers on a cube.
constexpr double kGripClosed = 0.75;
// The commanded joint angles close their gap to the IK solution with this
// time constant [s], without any joint turning faster than kMaxJointSpeed.
constexpr double kApproachTime = 0.08;
constexpr double kMaxJointSpeed = 2.0;
constexpr double kMaxGripSpeed = 1.5;
// The target stays above the ground and this close to the arm's base [m], so
// a drag towards the horizon cannot carry it out of sight.
constexpr double kMaxTargetDistance = 1.1;
// One lap of the --tour path [s].
constexpr double kTourPeriod = 12.0;
constexpr double kPi = 3.14159265358979323846;
// The IK frame is a point between the fingertips (see kinova_robotiq.urdf).
constexpr const char* kIkFrame = "grasp_frame_joint";
// Gripper pointing straight down, its grasp frame at (0.35, 0, 0.46). Higher
// or further out, a gripper pointing down nears the arm's singular poses, and
// small turns of the hand need large, slow joint motions or none at all.
using ArmPose = std::array<double, 6>;
constexpr ArmPose kHome{-0.0280036599681, 3.0295117382, -1.89610333104, 0.0, 1.35757023794, -0.0280036599555};
/// A pointer ray in world coordinates; `direction` has unit length.
struct Ray {
Eigen::Vector3d origin;
Eigen::Vector3d direction;
};
/// Distance between `point` and the ray (to its origin for points behind it).
inline double DistanceToRay(const Ray& ray, const Eigen::Vector3d& point) {
const double along = std::max(0.0, ray.direction.dot(point - ray.origin));
return (ray.origin + along * ray.direction - point).norm();
}
/// Moves a target with the pointer. The grabbed point stays under the pointer
/// on the plane through the target that faces the camera.
class TargetDrag {
public:
/// Grabs the target if the ray passes within `radius` of it. `view` is the
/// camera's unit viewing direction. Returns whether the target was grabbed.
bool Begin(const Ray& ray, const Eigen::Vector3d& target, const Eigen::Vector3d& view, double radius) {
if (DistanceToRay(ray, target) > radius) return false;
point_ = target;
normal_ = view;
Eigen::Vector3d hit;
if (!Intersect(ray, hit)) return false;
offset_ = target - hit;
active_ = true;
return true;
}
/// Target position for the pointer ray. Returns `target` unchanged when
/// nothing is grabbed or the ray misses the plane (parallel to it or
/// pointing away from it).
Eigen::Vector3d Move(const Ray& ray, const Eigen::Vector3d& target) const {
Eigen::Vector3d hit;
if (!active_ || !Intersect(ray, hit)) return target;
return hit + offset_;
}
/// Moves the plane, and with it the grabbed target, along the viewing
/// direction (away from the camera for positive distances).
void Push(double distance) { point_ += distance * normal_; }
void End() { active_ = false; }
bool active() const { return active_; }
private:
bool Intersect(const Ray& ray, Eigen::Vector3d& hit) const {
const double rate = ray.direction.dot(normal_);
if (std::abs(rate) < 1e-6) return false;
const double distance = (point_ - ray.origin).dot(normal_) / rate;
if (distance <= 0.0) return false;
hit = ray.origin + distance * ray.direction;
return true;
}
bool active_ = false;
Eigen::Vector3d point_ = Eigen::Vector3d::Zero();
Eigen::Vector3d normal_ = Eigen::Vector3d::UnitX();
Eigen::Vector3d offset_ = Eigen::Vector3d::Zero();
};
/// Turns `orientation` as if the pointer rolled it: moving (dx, dy) pixels
/// (y down) turns it about the camera's up axis and right axis, so its side
/// facing the camera follows the pointer.
inline Eigen::Matrix3d RotateByDrag(const Eigen::Matrix3d& orientation, double dx, double dy, const Eigen::Vector3d& right,
const Eigen::Vector3d& up, double radians_per_pixel) {
const Eigen::Quaterniond turn = Eigen::AngleAxisd(dy * radians_per_pixel, right.normalized()) *
Eigen::AngleAxisd(dx * radians_per_pixel, up.normalized());
return (turn * Eigen::Quaterniond(orientation)).normalized().toRotationMatrix();
}
/// The arm on its workbench with three cubes, and the controller that makes
/// the gripper follow a target pose.
///
/// SolveIk() runs ArticulatedSystem::solveIK() from the previous solution
/// without moving the simulated arm. Step() moves the arm's PD targets towards
/// that solution and integrates, so the arm stays a physical body that pushes,
/// grasps and lifts the cubes.
class Scene {
public:
explicit Scene(const std::string& model);
/// Sets the pose the grasp frame follows. The position is kept above the
/// ground and within kMaxTargetDistance of the base; a non-finite pose is
/// ignored.
void SetTarget(const Eigen::Vector3d& position, const Eigen::Matrix3d& orientation);
/// Follows the target's orientation too, or only its position. Switching it
/// on starts from the gripper's current orientation, so the hand does not
/// turn suddenly.
void SetTrackOrientation(bool track);
void SetGripperClosed(bool closed) { gripper_closed_ = closed; }
/// Solves IK for the target. Call when the target or the options changed.
void SolveIk();
/// Moves the PD targets towards the IK solution and integrates one step.
void Step();
/// Restores the initial state with the target at the home pose and the
/// gripper open.
void Reset();
/// Pose of the --tour path at `time`; it starts and ends at the home pose.
void TourPose(double time, Eigen::Vector3d& position, Eigen::Matrix3d& orientation) const;
/// Pose of the grasp frame.
void GraspPose(Eigen::Vector3d& position, Eigen::Matrix3d& orientation) const;
raisim::World& world() { return world_; }
raisim::ArticulatedSystem* robot() const { return robot_; }
const std::vector<raisim::Box*>& cubes() const { return cubes_; }
/// Options of the IK solve; Scene sets `seed`, `applySolution` and
/// `useOrientation` itself.
raisim::ArticulatedSystem::IKOptions& ik_options() { return ik_options_; }
const raisim::ArticulatedSystem::IKResult& ik_result() const { return ik_result_; }
const Eigen::VectorXd& ik_solution() const { return ik_solution_; }
const Eigen::VectorXd& command() const { return command_; }
const Eigen::VectorXd& command_velocity() const { return command_velocity_; }
const Eigen::Vector3d& target_position() const { return target_position_; }
const Eigen::Matrix3d& target_orientation() const { return target_orientation_; }
bool track_orientation() const { return track_orientation_; }
bool gripper_closed() const { return gripper_closed_; }
std::size_t arm_index(std::size_t joint) const { return arm_indices_[joint]; }
std::size_t grip_index() const { return grip_index_; }
private:
static constexpr std::array<double, 5> kFollowerSigns{-1.0, 1.0, -1.0, -1.0, 1.0};
/// Commands the home pose with the gripper open and makes it the IK seed.
void CommandHome();
raisim::World world_;
raisim::ArticulatedSystem* robot_ = nullptr;
std::vector<raisim::Box*> cubes_;
std::size_t ik_frame_ = 0;
std::array<std::size_t, 6> arm_indices_{};
std::array<std::size_t, 5> follower_indices_{};
std::size_t grip_index_ = 0;
raisim::ArticulatedSystem::IKOptions ik_options_;
raisim::ArticulatedSystem::IKResult ik_result_;
Eigen::VectorXd ik_solution_;
Eigen::VectorXd command_;
Eigen::VectorXd command_velocity_;
double grip_ = 0.0;
bool gripper_closed_ = false;
bool track_orientation_ = true;
Eigen::Vector3d home_position_ = Eigen::Vector3d::Zero();
Eigen::Matrix3d home_orientation_ = Eigen::Matrix3d::Identity();
Eigen::Vector3d target_position_ = Eigen::Vector3d::Zero();
Eigen::Matrix3d target_orientation_ = Eigen::Matrix3d::Identity();
raisim::World::Checkpoint checkpoint_;
};
inline Scene::Scene(const std::string& model) {
world_.setTimeStep(kTimeStep);
// A sleeping arm would not wake up for a new PD target.
world_.setSleepingEnabled(false);
world_.addGround(0.0, "ground")->setAppearance("checkerboard");
auto* table = world_.addBox(0.58, 0.74, 0.04, 1.0, "table");
table->setName("workbench");
table->setBodyType(raisim::BodyType::STATIC);
table->setPosition(0.43, 0.0, kTableHeight - 0.02);
table->setAppearance("0.32,0.36,0.42,1");
for (double x : {0.19, 0.67}) {
for (double y : {-0.31, 0.31}) {
auto* leg = world_.addBox(0.025, 0.025, kTableHeight - 0.04, 1.0);
leg->setBodyType(raisim::BodyType::STATIC);
leg->setPosition(x, y, 0.5 * (kTableHeight - 0.04));
leg->setAppearance("0.16,0.18,0.21,1");
}
}
raisim::ArticulatedSystemOption options;
options.convexifyCollisionMeshes = true;
robot_ = world_.addArticulatedSystem(model, "", {}, raisim::CollisionGroup(1), raisim::CollisionGroup(-1), options);
robot_->setName("kinova_robotiq");
if (robot_->getDOF() != 12 || robot_->getGeneralizedCoordinateDim() != 12) {
throw std::runtime_error("The fixed-base Kinova/Robotiq model must have twelve joints");
}
ik_frame_ = robot_->getFrameIdxByName(kIkFrame);
for (std::size_t i = 0; i < arm_indices_.size(); ++i) {
arm_indices_[i] = robot_->getGeneralizedVelocityIndex("kinova_joint_" + std::to_string(i + 1));
}
grip_index_ = robot_->getGeneralizedVelocityIndex("robotiq_85_left_knuckle_joint");
const std::array<const char*, 5> followers{"robotiq_85_right_knuckle_joint", "robotiq_85_left_inner_knuckle_joint", "robotiq_85_right_inner_knuckle_joint", "robotiq_85_left_finger_tip_joint",
"robotiq_85_right_finger_tip_joint"};
for (std::size_t i = 0; i < followers.size(); ++i) {
follower_indices_[i] = robot_->getGeneralizedVelocityIndex(followers[i]);
}
const std::size_t left_pad = robot_->getBodyIdx("robotiq_85_left_finger_tip_link");
const std::size_t right_pad = robot_->getBodyIdx("robotiq_85_right_finger_tip_link");
for (auto& body : robot_->getCollisionBodies()) {
body.setMaterial(body.localIdx == left_pad || body.localIdx == right_pad ? "pad" : "arm");
}
Eigen::VectorXd p_gain = Eigen::VectorXd::Zero(robot_->getDOF());
Eigen::VectorXd d_gain = p_gain;
const ArmPose p{2000.0, 3000.0, 2500.0, 600.0, 600.0, 300.0};
const ArmPose d{60.0, 80.0, 70.0, 20.0, 20.0, 12.0};
for (std::size_t i = 0; i < arm_indices_.size(); ++i) {
p_gain[arm_indices_[i]] = p[i];
d_gain[arm_indices_[i]] = d[i];
}
// Mimic followers stay passive, as in robotiq_pick_and_place.hpp.
p_gain[grip_index_] = 10.0;
d_gain[grip_index_] = 0.2;
robot_->setControlMode(raisim::ControlMode::PD_PLUS_FEEDFORWARD_TORQUE);
robot_->setPdGains(p_gain, d_gain);
robot_->setGeneralizedForce(Eigen::VectorXd::Zero(robot_->getDOF()));
const std::array<Eigen::Vector2d, 3> spots{Eigen::Vector2d(0.44, -0.16), Eigen::Vector2d(0.52, 0.0), Eigen::Vector2d(0.44, 0.16)};
const std::array<const char*, 3> colors{"0.85,0.22,0.22,1", "0.22,0.68,0.32,1", "0.22,0.42,0.88,1"};
for (std::size_t i = 0; i < spots.size(); ++i) {
auto* cube = world_.addBox(kCubeSize, kCubeSize, kCubeSize, 0.1, "cube");
cube->setName("cube_" + std::to_string(i));
cube->setPosition(spots[i].x(), spots[i].y(), kTableHeight + 0.5 * kCubeSize);
cube->setAppearance(colors[i]);
cubes_.push_back(cube);
}
world_.setMaterialPairProp("pad", "cube", 1.0, 0.0, 0.001);
world_.setMaterialPairProp("table", "cube", 0.8, 0.0, 0.001);
world_.setMaterialPairProp("arm", "cube", 0.5, 0.0, 0.001);
ik_options_.maxIterations = 50;
ik_options_.orientationTolerance = 1.0e-3;
// Trade 1 rad of hand orientation for 0.3 m of position when the target is
// out of reach.
ik_options_.orientationWeight = 0.3;
CommandHome();
robot_->setGeneralizedCoordinate(command_);
robot_->setGeneralizedVelocity(command_velocity_);
GraspPose(home_position_, home_orientation_);
target_position_ = home_position_;
target_orientation_ = home_orientation_;
// Let gravity settle the cubes, then keep this state for Reset().
for (int i = 0; i < 500; ++i) Step();
checkpoint_ = world_.captureCheckpoint();
}
inline void Scene::CommandHome() {
command_ = Eigen::VectorXd::Zero(robot_->getDOF());
command_velocity_ = command_;
for (std::size_t i = 0; i < arm_indices_.size(); ++i) command_[arm_indices_[i]] = kHome[i];
robot_->setPdTarget(command_, command_velocity_);
ik_solution_ = command_;
ik_result_ = raisim::ArticulatedSystem::IKResult();
ik_result_.success = true;
grip_ = 0.0;
gripper_closed_ = false;
}
inline void Scene::SetTarget(const Eigen::Vector3d& position, const Eigen::Matrix3d& orientation) {
if (!position.allFinite() || !orientation.allFinite()) return;
target_position_ = position;
target_position_.z() = std::max(target_position_.z(), 0.0);
const double distance = target_position_.norm();
if (distance > kMaxTargetDistance) target_position_ *= kMaxTargetDistance / distance;
target_orientation_ = orientation;
}
inline void Scene::SetTrackOrientation(bool track) {
if (track && !track_orientation_) {
Eigen::Vector3d position;
GraspPose(position, target_orientation_);
}
track_orientation_ = track;
}
inline void Scene::SolveIk() {
// Starting from the previous solution keeps the arm on one IK branch, and
// leaving the simulated state alone lets Step() move it physically.
ik_options_.seed.assign(ik_solution_.data(), ik_solution_.data() + ik_solution_.size());
ik_options_.applySolution = false;
ik_options_.useOrientation = track_orientation_;
raisim::Vec<3> position;
raisim::Mat<3, 3> orientation;
position.e() = target_position_;
orientation.e() = target_orientation_;
ik_result_ = robot_->solveIK(ik_frame_, position, orientation, ik_options_);
// Out of reach, the closest pose found is the best the arm can do.
const auto& solution = ik_result_.success ? ik_result_.solution : ik_result_.bestSolution;
if (solution.size() == static_cast<std::size_t>(ik_solution_.size())) {
ik_solution_ = Eigen::Map<const Eigen::VectorXd>(solution.data(), ik_solution_.size());
}
}
inline void Scene::Step() {
// Close the gap to the solution with time constant kApproachTime, scaling
// all joints together so the hand moves along a joint-space straight line.
Eigen::Matrix<double, 6, 1> speed;
for (std::size_t i = 0; i < arm_indices_.size(); ++i) {
speed[i] = (ik_solution_[arm_indices_[i]] - command_[arm_indices_[i]]) / kApproachTime;
}
const double fastest = speed.cwiseAbs().maxCoeff();
if (fastest > kMaxJointSpeed) speed *= kMaxJointSpeed / fastest;
for (std::size_t i = 0; i < arm_indices_.size(); ++i) {
command_[arm_indices_[i]] += speed[i] * kTimeStep;
command_velocity_[arm_indices_[i]] = speed[i];
}
const double grip_goal = gripper_closed_ ? kGripClosed : 0.0;
const double grip_speed = std::clamp((grip_goal - grip_) / kApproachTime, -kMaxGripSpeed, kMaxGripSpeed);
grip_ += grip_speed * kTimeStep;
command_[grip_index_] = grip_;
command_velocity_[grip_index_] = grip_speed;
// The followers' targets only document the linkage; they have no gains.
for (std::size_t i = 0; i < follower_indices_.size(); ++i) {
command_[follower_indices_[i]] = kFollowerSigns[i] * grip_;
command_velocity_[follower_indices_[i]] = kFollowerSigns[i] * grip_speed;
}
robot_->setPdTarget(command_, command_velocity_);
world_.integrate();
}
inline void Scene::Reset() {
std::string error;
if (!world_.restoreCheckpoint(checkpoint_, &error)) {
throw std::runtime_error("Cannot reset the IK scene: " + error);
}
CommandHome();
target_position_ = home_position_;
target_orientation_ = home_orientation_;
}
inline void Scene::TourPose(double time, Eigen::Vector3d& position, Eigen::Matrix3d& orientation) const {
// A figure eight that dips towards the table beside the cubes, with the
// hand turning about the vertical and tilting.
const double phase = 2.0 * kPi * time / kTourPeriod;
position = home_position_ + Eigen::Vector3d(0.08 * std::sin(2.0 * phase), 0.18 * std::sin(phase), 0.03 * (std::cos(2.0 * phase) - 1.0));
orientation = (Eigen::AngleAxisd(0.6 * std::sin(phase), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.3 * std::sin(2.0 * phase), Eigen::Vector3d::UnitY())).toRotationMatrix() *
home_orientation_;
}
inline void Scene::GraspPose(Eigen::Vector3d& position, Eigen::Matrix3d& orientation) const {
raisim::Vec<3> p;
raisim::Mat<3, 3> r;
robot_->getFramePosition(ik_frame_, p);
robot_->getFrameOrientation(ik_frame_, r);
position = p.e();
orientation = r.e();
}
} // namespace raisim_examples::arm_ik
#endif // RAISIM_EXAMPLES_INCLUDE_ARM_IK_DEMO_HPP_Related
- Robotiq Gripper Mimic: the same arm and gripper in a scripted pick and place.
- Dynamics and Control: joint PD control and its torque limits.
ArticulatedSystem::IKResult: the residuals and solutions the panel reports.