{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Credit" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "This example is a (slightly modified) contribution by [Aykut Satici](https://github.com/symplectomorphism)." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Setup" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We'll be using `LinearAlgebra`, `RigidBodyDynamics`, and `StaticArrays` for the simulation part." ] }, { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [], "source": [ "using LinearAlgebra\n", "using RigidBodyDynamics\n", "using StaticArrays" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Model definition" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We're going to create a [four-bar linkage](https://en.wikipedia.org/wiki/Four-bar_linkage) that looks like this:\n", "\n", "\n", "We'll 'cut' the mechanism at joint 4: joints 1, 2, and 3 will be part of the spanning tree of the mechanism, but joint 4 will be a 'loop joint' (see e.g. Featherstone's 'Rigid Body Dynamics Algorithms'), for which the dynamics will be enforced using Lagrange multipliers." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "First, we'll define some relevant constants:" ] }, { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "# gravitational acceleration\n", "g = -9.81\n", "\n", "# link lengths\n", "l_0 = 1.10\n", "l_1 = 0.5\n", "l_2 = 1.20\n", "l_3 = 0.75\n", "\n", "# link masses\n", "m_1 = 0.5\n", "m_2 = 1.0\n", "m_3 = 0.75\n", "\n", "# link center of mass offsets from the preceding joint axes\n", "c_1 = 0.25\n", "c_2 = 0.60\n", "c_3 = 0.375\n", "\n", "# moments of inertia about the center of mass of each link\n", "I_1 = 0.333\n", "I_2 = 0.537\n", "I_3 = 0.4\n", "\n", "# scalar type\n", "T = Float64;\n", "\n", "# Rotation axis: negative y-axis\n", "axis = SVector(zero(T), -one(T), zero(T));" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Construct the world rigid body and create a new mechanism." ] }, { "cell_type": "code", "execution_count": 3, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "Spanning tree:\n", "Vertex: world (root)\n", "No non-tree joints." ] }, "execution_count": 3, "metadata": {}, "output_type": "execute_result" } ], "source": [ "world = RigidBody{T}(\"world\")\n", "fourbar = Mechanism(world; gravity = SVector(0., 0., g))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Next, we'll construct the spanning tree of the mechanism, consisting of bodies 1, 2, and 3 connected by joints 1, 2, and 3." ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "Spanning tree:\n", "Vertex: world (root)\n", " Vertex: inertia1_centroidal, Edge: joint1\n", " Vertex: inertia2_centroidal, Edge: joint2\n", " Vertex: inertia3_centroidal, Edge: joint3\n", "No non-tree joints." ] }, "execution_count": 4, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# link1 and joint1\n", "joint1 = Joint(\"joint1\", Revolute(axis))\n", "inertia1 = SpatialInertia(CartesianFrame3D(\"inertia1_centroidal\"), I_1*axis*axis', zero(SVector{3, T}), m_1)\n", "link1 = RigidBody(inertia1)\n", "before_joint1_to_world = one(Transform3D, frame_before(joint1), default_frame(world))\n", "c1_to_joint = Transform3D(inertia1.frame, frame_after(joint1), SVector(c_1, 0, 0))\n", "attach!(fourbar, world, link1, joint1, joint_pose = before_joint1_to_world, successor_pose = c1_to_joint)\n", "\n", "# link2 and joint2\n", "joint2 = Joint(\"joint2\", Revolute(axis))\n", "inertia2 = SpatialInertia(CartesianFrame3D(\"inertia2_centroidal\"), I_2*axis*axis', zero(SVector{3, T}), m_2)\n", "link2 = RigidBody(inertia2)\n", "before_joint2_to_after_joint1 = Transform3D(frame_before(joint2), frame_after(joint1), SVector(l_1, 0., 0.))\n", "c2_to_joint = Transform3D(inertia2.frame, frame_after(joint2), SVector(c_2, 0, 0))\n", "attach!(fourbar, link1, link2, joint2, joint_pose = before_joint2_to_after_joint1, successor_pose = c2_to_joint)\n", "\n", "# link3 and joint3\n", "joint3 = Joint(\"joint3\", Revolute(axis))\n", "inertia3 = SpatialInertia(CartesianFrame3D(\"inertia3_centroidal\"), I_3*axis*axis', zero(SVector{3, T}), m_3)\n", "link3 = RigidBody(inertia3)\n", "before_joint3_to_world = Transform3D(frame_before(joint3), default_frame(world), SVector(l_0, 0., 0.))\n", "c3_to_joint = Transform3D(inertia3.frame, frame_after(joint3), SVector(c_3, 0, 0))\n", "attach!(fourbar, world, link3, joint3, joint_pose = before_joint3_to_world, successor_pose = c3_to_joint)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Finally, we'll add joint 4 in almost the same way we did the other joints, with the following exceptions:\n", "1. both `link2` and `link3` are already part of the `Mechanism`, so the `attach!` function will figure out that `joint4` will be a loop joint.\n", "2. instead of using the default (identity) for the argument that specifies the transform from the successor of joint 4 (i.e., link 3) to the frame directly after joint 4, we'll specify a transform that incorporates the $l_3$ offset." ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "Spanning tree:\n", "Vertex: world (root)\n", " Vertex: inertia1_centroidal, Edge: joint1\n", " Vertex: inertia2_centroidal, Edge: joint2\n", " Vertex: inertia3_centroidal, Edge: joint3\n", "Non-tree joints:\n", "joint4, predecessor: inertia2_centroidal, successor: inertia3_centroidal" ] }, "execution_count": 5, "metadata": {}, "output_type": "execute_result" } ], "source": [ "# joint between link2 and link3\n", "joint4 = Joint(\"joint4\", Revolute(axis))\n", "before_joint4_to_joint2 = Transform3D(frame_before(joint4), frame_after(joint2), SVector(l_2, 0., 0.))\n", "joint3_to_after_joint4 = Transform3D(frame_after(joint3), frame_after(joint4), SVector(-l_3, 0., 0.))\n", "attach!(fourbar, link2, link3, joint4, joint_pose = before_joint4_to_joint2, successor_pose = joint3_to_after_joint4)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Note the additional non-tree joint in the printed `Mechanism` summary." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Simulation" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "As usual, we'll now construct a `MechanismState` and `DynamicsResult` for the four-bar `Mechanism`. We'll set some initial conditions for a simulation, which were solved for a priori using a nonlinear program (not shown here)." ] }, { "cell_type": "code", "execution_count": 6, "metadata": {}, "outputs": [], "source": [ "state = MechanismState(fourbar)\n", "result = DynamicsResult(fourbar);" ] }, { "cell_type": "code", "execution_count": 7, "metadata": {}, "outputs": [ { "data": { "text/plain": [ "true" ] }, "execution_count": 7, "metadata": {}, "output_type": "execute_result" } ], "source": [ "set_configuration!(state, joint1, 1.6707963267948966) # θ\n", "set_configuration!(state, joint2, -1.4591054166649482) # γ\n", "set_configuration!(state, joint3, 1.5397303602625536) # ϕ\n", "\n", "set_velocity!(state, joint1, 0.5)\n", "set_velocity!(state, joint2, -0.47295)\n", "set_velocity!(state, joint3, 0.341)\n", "\n", "# Invalidate the cache variables\n", "setdirty!(state)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Next, we'll do a 3-second simulation:" ] }, { "cell_type": "code", "execution_count": 8, "metadata": {}, "outputs": [], "source": [ "ts, qs, vs = simulate(state, 3., Δt = 1e-2);" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# Visualization" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "For visualization, we'll use [`RigidBodyTreeInspector`](https://github.com/rdeits/RigidBodyTreeInspector.jl).\n", "\n", "(Note: the `#NBSKIP` comments are to skip these cells during `Pkg.test(\"RigidBodyDynamics\")`)" ] }, { "cell_type": "code", "execution_count": 9, "metadata": { "scrolled": true }, "outputs": [ { "data": { "text/html": [ "
\n", " \n", " \n", "
" ] }, "metadata": {}, "output_type": "display_data" }, { "name": "stderr", "output_type": "stream", "text": [ "\u001b[1m\u001b[36mINFO: \u001b[39m\u001b[22m\u001b[36mInteract.jl: using new nbwidgetsextension protocol\n", "\u001b[39m" ] } ], "source": [ "#NBSKIP\n", "using RigidBodyTreeInspector" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Open the viewer application if it isn't open already:" ] }, { "cell_type": "code", "execution_count": 10, "metadata": {}, "outputs": [], "source": [ "#NBSKIP\n", "DrakeVisualizer.any_open_windows() || (DrakeVisualizer.new_window(); sleep(1));" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Load the mechanism's geometry into the visualizer:" ] }, { "cell_type": "code", "execution_count": 11, "metadata": {}, "outputs": [ { "name": "stderr", "output_type": "stream", "text": [ "2018-03-21 00:45:56.538 drake-visualizer[40259:6478725] *** WARNING: Method userSpaceScaleFactor in class NSView is deprecated on 10.7 and later. It should not be used in new applications. Use convertRectToBacking: instead. \n" ] } ], "source": [ "#NBSKIP\n", "vis = DrakeVisualizer.Visualizer()[:robot1];\n", "RigidBodyTreeInspector.setgeometry!(vis, fourbar, visual_elements(fourbar, Skeleton(inertias = false)));" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "And animate:" ] }, { "cell_type": "code", "execution_count": 12, "metadata": {}, "outputs": [], "source": [ "#NBSKIP\n", "RigidBodyTreeInspector.animate(vis, fourbar, ts, qs; realtimerate = 1.);" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "Julia 0.6.2", "language": "julia", "name": "julia-0.6" }, "language_info": { "file_extension": ".jl", "mimetype": "application/julia", "name": "julia", "version": "0.6.2" } }, "nbformat": 4, "nbformat_minor": 2 }