├── .github └── workflows │ └── generate_rig.yml ├── .gitignore ├── CHANGELOG.md ├── LICENSE ├── README.md ├── doc └── faq.md ├── rigs ├── iCubBlenderV2_5.blend ├── iCubBlenderV2_5_plus.blend ├── iCubBlenderV2_5_visuomanip.blend ├── iCubBlenderV2_6.blend ├── iCubBlenderV2_7.blend └── iCubBlenderV3.blend └── script ├── addons_installer.py ├── blenderRCBPanel ├── __init__.py ├── blenderRCBPanel.py └── common_functions.py ├── conf └── parts.json └── urdfToBlender ├── __init__.py └── urdfToBlender.py /.github/workflows/generate_rig.yml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: Automatically generate rigs from icub-models urdf 4 | 5 | # Controls when the workflow will run 6 | on: 7 | repository_dispatch: 8 | types: [repository_trigger] 9 | 10 | # A workflow run is made up of one or more jobs that can run sequentially or in parallel 11 | jobs: 12 | # This workflow contains a single job called "build" 13 | generate_rig: 14 | # The type of runner that the job will run on 15 | runs-on: ubuntu-latest 16 | 17 | # Steps represent a sequence of tasks that will be executed as part of the job 18 | steps: 19 | 20 | - uses: conda-incubator/setup-miniconda@v2 21 | with: 22 | mamba-version: "*" 23 | channels: conda-forge,robotology 24 | channel-priority: true 25 | 26 | - name: Dependencies 27 | shell: bash -l {0} 28 | run: | 29 | # Workaround for https://github.com/conda-incubator/setup-miniconda/issues/186 30 | conda config --remove channels defaults 31 | # Actual dependencies 32 | mamba install python=3.8 yarp idyntree 33 | # Install Blender 34 | sudo apt-get update 35 | sudo apt-get install blender 36 | 37 | - name: Install icub-models 38 | id: get_icub-models 39 | run: | 40 | git clone https://github.com/robotology/icub-models 41 | cd icub-models 42 | commit_hash=$(git rev-parse HEAD) 43 | mkdir build && cd build 44 | cmake -DCMAKE_INSTALL_PREFIX=/home/runner/install .. 45 | make install 46 | echo "commit_hash=${commit_hash}" >> $GITHUB_OUTPUT 47 | 48 | # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it 49 | - uses: actions/checkout@v2 50 | 51 | - name: Run conversion script 52 | run: | 53 | cd ${GITHUB_WORKSPACE} 54 | export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:/home/runner/install/share/iCub/robots:/home/runner/install/share 55 | export PYTHONPATH=/usr/share/miniconda/envs/test/lib/python3.8/site-packages 56 | models_list="\"$(echo ${{ github.event.client_payload.models_list }} > temp.tmp && sed -i 's/ /" "/g' temp.tmp && cat temp.tmp)\"" 57 | rm temp.tmp 58 | models_list=( $models_list ) 59 | for _model in "${models_list[@]}" 60 | do 61 | echo "For loop!" 62 | echo $_model 63 | _version=$(echo $_model | grep -o -P '(?<=iCubGazeboV).*(?=/model)') 64 | echo $_version 65 | blender --python-use-system-env -b -P "./script/urdfToBlender/urdfToBlender.py" -- --urdf_filename /home/runner/install/share/iCub/robots/iCubGazeboV${_version}/model.urdf --blend_filename ./rigs/iCubBlenderV${_version}.blend 66 | done 67 | ls -la ./rigs 68 | 69 | - name: Commit and push changes 70 | run: | 71 | cd ${GITHUB_WORKSPACE} 72 | git status 73 | git config --local user.email "actions@github.com" 74 | git config --local user.name "GitHub Action" 75 | git add -A 76 | git commit -m "rigs: automatic update" -m "icub-models commit: ${{ steps.get_icub-models.outputs.commit_hash }}" 77 | git fetch origin 78 | git rebase origin/master 79 | git push origin master 80 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.blend1 -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | All notable changes to this project will be documented in this file. 4 | 5 | The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), 6 | and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). 7 | 8 | ## [Unreleased] 9 | 10 | ## [0.5.0] - 2022-08-31 11 | 12 | ### `blenderRCBPanel` 13 | 14 | - Fixed when the urdf model is not saved inside the scene. 15 | - Removed from the list of joints the ones controlled via drivers. 16 | 17 | ## [0.4.0] - 2022-05-26 18 | 19 | ### `blenderRCBPanel` 20 | 21 | - Added list of controllable joints for designing animations. 22 | - Added section for reaching a cartesian target. 23 | - Added Drag&Drop function for moving in the cartesian space. 24 | 25 | ## [0.3.0] - 2022-02-28 26 | 27 | ### `addons_installer` 28 | 29 | - Added python script for installing and enabling the blender addons of this 30 | repository. 31 | 32 | ### `urdfToBlender` 33 | 34 | - Code refactored for displaying the converter as panel that can be installed as addon. 35 | 36 | ### `blenderRCBPanel` 37 | 38 | - Added robot's parts configuration through a JSON file structured as the [proposed template](https://github.com/robotology/blender-robotics-utils/blob/master/script/conf/parts.json) 39 | - Code refactored to be able to display the panel in the list of add-ons of blender 40 | 41 | ## [0.2.0] - 2021-11-29 42 | 43 | - Added action for automatically generate the rigs every time a commit 44 | is made in [`icub-models`](https://github.com/robotology/icub-models) 45 | 46 | ### `urdfToBlender` 47 | 48 | - Added the support in urdfToBlender for the basic geometries 49 | - Added the possibility to run it headless. 50 | 51 | ### `rigs` 52 | 53 | - Added `iCubBlenderV2_5_visuomanip.blend`. 54 | 55 | ### `blenderRCBPanel` 56 | 57 | - Added changes for controlling iCub hands. 58 | 59 | ## [0.1.0] - 2021-08-30 60 | 61 | - Added `blenderRCBPanel` python script that spawns a panel for controlling parts of 62 | the robot(for now tested only with iCub). 63 | - Added `urdfToBlender` python script that creates a rig starting from a urdf of a robot. 64 | - Added `iCubBlenderV2_5.blend` and `iCubBlenderV3.blend`. 65 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) Fondazione Istituto Italiano di Tecnologia 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions 6 | are met: 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | 3. Neither the name of the University nor the names of its contributors 13 | may be used to endorse or promote products derived from this software 14 | without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND 17 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22 | OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25 | OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26 | SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Blender Robotics Utils 2 | 3 | This repository contains utilities for exporting/controlling your robot in [Blender](https://www.blender.org/) 4 | 5 | ![ezgif com-gif-maker](https://user-images.githubusercontent.com/19152494/128324719-b9bda13d-92dd-49f5-b866-8dd04b3f9d76.gif) 6 | 7 | ## Maintainers 8 | 9 | This repository is maintained by: 10 | 11 | | | | | 12 | |:---:|:---:|:---:| 13 | [](https://github.com/niNicogenecogene) | [@Nicogene](https://github.com/Nicogene) | | 14 | 15 | ## Addons installation 16 | 17 | ### Dependencies 18 | 19 | - Blender > 2.79 and < 4.0 (see https://github.com/robotology/blender-robotics-utils/issues/30) 20 | - [iDynTree](https://github.com/robotology/idyntree) python bindings 21 | - `GAZEBO_MODEL_PATH` [properly set](https://github.com/robotology/icub-models#use-the-models-with-gazebo). 22 | 23 | An easy way to install the dependencies is to use the [conda](https://docs.conda.io/en/latest/) binaries packages. 24 | Just [install conda](https://github.com/robotology/robotology-superbuild/blob/master/doc/install-miniforge.md) and then: 25 | 26 | ```console 27 | conda create -n blenderenv 28 | conda activate blenderenv 29 | conda install -c conda-forge -c robotology python= yarp idyntree 30 | conda env config vars set PYTHONPATH=/where/the/bindings/are/installed 31 | ``` 32 | 33 | where `` is the python version used inside Blender. 34 | 35 | ### Installation 36 | 37 | The addons can be installed by running the following command: 38 | 39 | ```console 40 | conda activate blenderenv 41 | cd blender-robotics-utils/script 42 | blender -b --python-use-system-env -P ./addons_installer.py 43 | ``` 44 | 45 | ## urdfToBlender 46 | 47 | Panel/Python script that given the urdf of a robot as input, define the complete rig, in terms of bones, meshes and joint limits. 48 | 49 | ### Usage with GUI 50 | 51 | Once installed correctly the dependencies run: 52 | 53 | (Windows Powershell) 54 | 55 | ```console 56 | & "C:\Program Files\Blender Foundation\Blender \blender.exe" --python-use-system-env 57 | ``` 58 | 59 | (Linux & macOs) 60 | 61 | ```console 62 | blender --python-use-system-env 63 | ``` 64 | 65 | If the installation went fine you should have this panel on the right under the `Tools` section: 66 | 67 | ![immagine](https://user-images.githubusercontent.com/19152494/154102335-76c5312a-81ea-46b5-92cc-93d0668596e7.png) 68 | 69 | After clicking "Select the urdf" it will be opened a file browse such as: 70 | 71 | ![immagine](https://user-images.githubusercontent.com/19152494/126337119-6b899183-1f2a-413c-8b88-4e5727818891.png) 72 | 73 | After selecting the urdf, the script creates the rig of the robot in term of armature and meshes. 74 | 75 | ### Usage without GUI 76 | 77 | It is also possible to run this script from the command line interface, in this case you have to specify the `urdf_fiename` 78 | to be converted and optionally the `blend_filename` to be saved(by default it saves `robot.blend` in the current directory). 79 | 80 | (Windows Powershell) 81 | 82 | ```console 83 | "C:\Program Files\Blender Foundation\Blender \blender.exe" --python-use-system-env -b -P "C:\where\you\have\blender-robotics-utils\script\urdfToBlender.py" -- --urdf_filename "C:\where\you\have\model.urdf" --blend_filename "C:\where\you\want\to\save\myrobot.blend" 84 | ``` 85 | 86 | (Linux & macOs) 87 | 88 | ```console 89 | blender --python-use-system-env -b -P "/where/you/have/blender-robotics-utils/script/urdfToBlender.py" -- --urdf_filename "/where/you/have/model.urdf" --blend_filename "/where/you/want/to/save/myrobot.blend" 90 | ``` 91 | 92 | ### Examples 93 | 94 | |**iCub 2.5** | **iCub 3**| 95 | |:---:|:---:| 96 | | ![immagine](https://user-images.githubusercontent.com/19152494/126991916-39b97bd1-da3b-4114-8597-9d835ad835a1.png) | ![immagine](https://user-images.githubusercontent.com/19152494/126991957-feb4eb6b-5ae0-4d3b-bfef-4ec05a5eaf10.png) | 97 | 98 | ### Known limitations 99 | 100 | - Only fixed or revolute joints are handled(see https://github.com/robotology/idyntree/issues/881, it requires iDynTree >= 3.3.0). 101 | - Only `.stl`, `.ply` and `.dae` format are supported for meshes. 102 | 103 | 104 | ## blenderRCBPanel 🚧 105 | 106 | Python addon that consists in a panel inside the pose mode for connecting parts of the rig to the parts of the real robot(or simulator). 107 | 108 | If the installation went fine you should have this panel on the right under the `Tools` section. 109 | First of all you have to configure it loading a `.json` file representing the structure of your robot like this one: 110 | 111 | ```json 112 | { 113 | "parts": [ 114 | ["torso", "Torso"], 115 | ["head", "Head"], 116 | ["left_arm", "Left arm"], 117 | ["right_arm", "Right arm"], 118 | ["left_leg", "Left leg"], 119 | ["right_leg", "Right leg"] 120 | ] 121 | } 122 | ``` 123 | 124 | It should contain a list of pair where the first value will be the "YARP name" of the part, and the second one will be the name displayed in the list. 125 | Once configured, select the parts you want to control, press connect and then have fun! 126 | This has been tested with `iCub 2.5`. 127 | 128 | ### Joint space 129 | 130 | It is possible to define the animation changing the values of joints from the joints' list, every time a new value is entered a waypoint in the animation is setted. 131 | 132 | Video 🎥: 133 | 134 | https://user-images.githubusercontent.com/19833605/159922346-0bc9cd53-1a5a-4ea1-a7f7-453bdbdc1547.mp4 135 | 136 | ### Cartesian space 137 | 138 | #### Reach target 139 | 140 | 1. The user selects the `Base Frame` and the `End Effector Frame` according to the joint he/she wants to move. 141 | 2. Input the tranformation to be reached by `End Effector Frame` respect to `Base Frame`, in XYZ(meters) and RPY(degrees). 142 | 3. Press `Reach target` button. 143 | 4. The waypoint in the animation is automatically set. 144 | 145 | Video 🎥: 146 | 147 | https://user-images.githubusercontent.com/19152494/165498930-224c3871-620a-4c6c-9162-7e30c3578265.mp4 148 | 149 | #### Drag & Drop 150 | 151 | 1. The user selects the `Base Frame` and the `End Effector Frame` according to the joint he/she wants to move. 152 | 2. Press the button `Drag & Drop` to activate the feature. Once the button is pressed the user loses control of Blender. 153 | 3. The user moves the mouse pointer in the 3D space of Blender and clicks the `right mouse button` to drop the position. 154 | 4. The user clicks on the `left mouse button` to deactivate the drag and drop feature and to bring back the control to Blender. 155 | 5. The waypoint in the animation is automatically set. 156 | 157 | Video 🎥: 158 | 159 | https://user-images.githubusercontent.com/19833605/167880668-5176a0c1-3110-41dc-be9f-8e0565752430.mp4 160 | 161 | ### Known limitations 162 | 163 | - We are controlling sequentially all the parts connected, this may lead to some discrepancies between the animation and the movements. This can be improved using multithreading and/or using a remapper. 164 | 165 | ## FAQs 🙋‍♂️ 166 | 167 | Check our [faq section](https://github.com/robotology/blender-robotics-utils/blob/master/doc/faq.md#frequentely-asked-questions) 👈 168 | -------------------------------------------------------------------------------- /doc/faq.md: -------------------------------------------------------------------------------- 1 | # Frequentely Asked Questions 2 | 3 | ## How can I edit the speed of an animation? 4 | 5 | After setting the keyframes of your animation, you can change the `animation speed` acting on the keyframes visible in the `Dope Sheet` panel. 6 | The `Dope Sheet` panel gives you a complete overview of the keyframes and the joints involved in the animation. You can access the `Dope Sheet` panel from the top left menu called `Editor Type`, select `Animation` -> `Dope Sheet`. 7 | If you click on the keyframes in the `iCub` or `iCubAction` rows you will select the keyframes for all the joints. You can refine the keyframe for the single joint by clicking on the corresponding row. 8 | 9 | - The keyframes will appear yellow when they are selected, and white when de-selected. 10 | - You will de-select the keyframes by clicking anywhere inside the timeline. 11 | - Select with the mouse the desired keyframe. 12 | - To select multiple keyframes, select the keyframes and press `Shift` at the same time. 13 | - To select all the keyframes press `A`. 14 | - If you drag the keyframe toward left or right you will change the animation speed relative to the selected keyframe. 15 | - To move multiple keyframes, select the keyframes you want to move and press `G`. 16 | - To scale the selected keyframe press `S`. 17 | - To extend the time between two keyframes, select all the keyframes (press `A`) place the mouse cursor between two keyframes, and press `E`. 18 | 19 | 20 | The following video shows some of the operations described above. 21 | 22 | 23 | https://user-images.githubusercontent.com/19833605/168836359-5c2158c9-cbb2-40d0-9231-21f5baad3cf4.mp4 24 | 25 | ## How can I edit the trajectory shape of the animation? 26 | 27 | You can also adjust the animation curves through the `Graph Editor`. The Graph Editor displays all the trajectory curves for the animation of all the joints. You can modify the animation of the single joint to have a refined behavior. You can access the Graph Editor panel from the top-left menu `Editor Type`, select `Animation` -> `Graph Editor`. 28 | 29 | - You can select the joint you want to modify from the `3D Viewport` window, the Graph Editor will display the curves the joint follows during the animation. You can directly modify the curve by dragging and dropping the keyframe. 30 | - If you select the entire robot from the `3D Viewport` window (press `A`) you will see all the curves that the joints follow, again select a keyframe and you will modify the curve for all the joints. 31 | - You can also modify the curve for the single keyframe by selecting a keyframe from the `Timeline` pane, right-click with the mouse button, choosing `Interpolation Mode`, and then choosing the desired effect. 32 | 33 | The following video shows some of the operations described above. 34 | 35 | https://user-images.githubusercontent.com/19833605/168836429-bf6d2f1f-cd3e-456c-bddc-c8fcc8a625dc.mp4 36 | 37 | ## How can I record the trajectories of the joint (without using Gazebo) ? 38 | 39 | - First execute a yarp fakeMotionControl device, configured to work with the set of joints that you need (the names must match the joints name in the urdf opened by Blender): 40 | ``` 41 | yarpdev --device fakeMotionControl --name "/iCub/my_custom_set_of_joints" --GENERAL::Joints 3 --GENERAL::AxisName "(neck_pitch neck_roll torso_yaw)" --period 0.010 42 | ``` 43 | Note that the above command line demonstrates the possibility of mixing joints belonging to different parts of the robot body (head, torso). 44 | 45 | - Connect the blenderRCB plugin to the robot part opened by fakeMotionControl. In the example above, you need to have the part `my_custom_set_of_joints` specified in the jason file loaded by blenderRCB plugin. 46 | - Read the joints position from the yarp port opened by the fakeMotion control, i.e.: 47 | ``` 48 | yarp read ... //iCub/my_custom_set_of_joints/stete:o envelope &> traj.txt 49 | ``` 50 | The file will have the following aspect: 51 | ``` 52 | 001 1733307092.852 0.0 0.0 0.0 53 | 002 1733307092.862 0.0 0.0 0.0 54 | 003 1733307092.873 0.0 0.0 0.0 55 | 004 1733307092.883 0.0 0.0 0.0 56 | ``` 57 | The first two values are the timestamp (a progressive counter and a time measured in seconds), than the second group of numbers are the encoder values of the three joints (indeed the fakeMotionControl instantaneously assigns to the encoder the value received as positionDirect command from the blender plugin). Please note that the trajectory is sampled with the frequency specified by the `--period` option of the fakeMotionControl (the recommended value is 10 milliseconds, as show in the example) 58 | - You might also want to use different tools to record the encoders values, such as [yarpdatadumper](https://www.yarp.it/latest/group__yarpdatadumper.html) 59 | 60 | ## How can I replay on the robot a previosuly recorded trajectory? 61 | 62 | TO BE COMPLETED 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /rigs/iCubBlenderV2_5.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/blender-robotics-utils/054b75a7919ad536b75f27fd090a6dc0edb1578f/rigs/iCubBlenderV2_5.blend -------------------------------------------------------------------------------- /rigs/iCubBlenderV2_5_plus.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/blender-robotics-utils/054b75a7919ad536b75f27fd090a6dc0edb1578f/rigs/iCubBlenderV2_5_plus.blend -------------------------------------------------------------------------------- /rigs/iCubBlenderV2_5_visuomanip.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/blender-robotics-utils/054b75a7919ad536b75f27fd090a6dc0edb1578f/rigs/iCubBlenderV2_5_visuomanip.blend -------------------------------------------------------------------------------- /rigs/iCubBlenderV2_6.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/blender-robotics-utils/054b75a7919ad536b75f27fd090a6dc0edb1578f/rigs/iCubBlenderV2_6.blend -------------------------------------------------------------------------------- /rigs/iCubBlenderV2_7.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/blender-robotics-utils/054b75a7919ad536b75f27fd090a6dc0edb1578f/rigs/iCubBlenderV2_7.blend -------------------------------------------------------------------------------- /rigs/iCubBlenderV3.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/robotology/blender-robotics-utils/054b75a7919ad536b75f27fd090a6dc0edb1578f/rigs/iCubBlenderV3.blend -------------------------------------------------------------------------------- /script/addons_installer.py: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2022 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | 8 | import bpy 9 | from distutils import dir_util 10 | import os 11 | 12 | addon_path = bpy.utils.user_resource('SCRIPTS', path="addons", create=True) 13 | print("Installing blender addons in", addon_path) 14 | addon_urdfToBlender = os.path.join(addon_path, 'urdfToBlender') 15 | addon_blenderRCBPanel = os.path.join(addon_path, 'blenderRCBPanel') 16 | 17 | origin_path_urdfToBlender = os.path.abspath('urdfToBlender/') 18 | origin_path_blenderRCBPanel = os.path.abspath('blenderRCBPanel/') 19 | 20 | dir_util.copy_tree(origin_path_urdfToBlender, addon_urdfToBlender) 21 | dir_util.copy_tree(origin_path_blenderRCBPanel, addon_blenderRCBPanel) 22 | 23 | bpy.ops.preferences.addon_enable(module='urdfToBlender') 24 | bpy.ops.preferences.addon_enable(module='blenderRCBPanel') 25 | bpy.ops.wm.save_userpref() -------------------------------------------------------------------------------- /script/blenderRCBPanel/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2022 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | bl_info = { 8 | "name": "RemoteControlBoard Panel", 9 | "description": "Panel for attaching the remote_controlboard of a YARP-based robot", 10 | "author": "Nicogene", 11 | "version": (0, 0, 1), 12 | "blender": (2, 80, 0), 13 | "location": "3D View > Tools", 14 | "warning": "", # used for warning icon and text in addons panel 15 | "wiki_url": "", 16 | "tracker_url": "", 17 | "category": "Development" 18 | } 19 | 20 | import bpy 21 | 22 | from bpy.props import (StringProperty, 23 | BoolProperty, 24 | IntProperty, 25 | FloatProperty, 26 | FloatVectorProperty, 27 | EnumProperty, 28 | PointerProperty, 29 | CollectionProperty 30 | ) 31 | 32 | from .blenderRCBPanel import (MyProperties, 33 | WM_OT_Disconnect, 34 | WM_OT_Connect, 35 | WM_OT_Configure, 36 | WM_OT_ReachTarget, 37 | WM_OT_initiate_drag_drop, 38 | ModalOperator, 39 | OBJECT_PT_robot_controller, 40 | OT_OpenConfigurationFile, 41 | ListItem, 42 | MY_UL_List, 43 | ) 44 | 45 | # ------------------------------------------------------------------------ 46 | # Registration 47 | # ------------------------------------------------------------------------ 48 | 49 | classes = ( 50 | MyProperties, 51 | WM_OT_Disconnect, 52 | WM_OT_Connect, 53 | WM_OT_Configure, 54 | WM_OT_ReachTarget, 55 | WM_OT_initiate_drag_drop, 56 | ModalOperator, 57 | OBJECT_PT_robot_controller, 58 | OT_OpenConfigurationFile, 59 | ListItem, 60 | MY_UL_List 61 | ) 62 | 63 | 64 | def register(): 65 | for cls in classes: 66 | try: 67 | bpy.utils.register_class(cls) 68 | except: 69 | print("an exception when registering the class") 70 | 71 | try: 72 | bpy.types.Scene.my_tool = PointerProperty(type=MyProperties) 73 | bpy.types.Scene.my_list = CollectionProperty(type=ListItem) 74 | bpy.types.Scene.list_index = IntProperty(name="Index for my_list", default=0) 75 | 76 | except: 77 | print("A problem in the registration occurred") 78 | 79 | # initialize the dict 80 | bpy.types.Scene.rcb_wrapper = {} 81 | 82 | 83 | def unregister(): 84 | for cls in reversed(classes): 85 | try: 86 | bpy.utils.unregister_class(cls) 87 | except: 88 | print("An exception was raised when unregistering the class") 89 | try: 90 | del bpy.types.Scene.my_tool 91 | del bpy.types.Scene.my_list 92 | del bpy.types.Scene.list_index 93 | except: 94 | print("Exception raised when deleting the scene.") 95 | 96 | try: 97 | # remove the callback 98 | bpy.app.handlers.frame_change_post.clear() 99 | except: 100 | print("Exception raised when removing the callback") 101 | 102 | 103 | if __name__ == "__main__": 104 | register() 105 | -------------------------------------------------------------------------------- /script/blenderRCBPanel/blenderRCBPanel.py: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | from ntpath import join 8 | import bpy 9 | import os 10 | # import sys 11 | import yarp 12 | import idyntree.bindings as iDynTree 13 | # import numpy as np 14 | import math 15 | import json 16 | from .common_functions import (printError, 17 | look_for_bones_with_drivers, 18 | bones_with_driver, 19 | IkVariables as ikv, 20 | InverseKinematics, 21 | ) 22 | 23 | from bpy_extras.io_utils import ImportHelper 24 | from bpy_extras import view3d_utils 25 | 26 | from bpy.props import (StringProperty, 27 | BoolProperty, 28 | IntProperty, 29 | FloatProperty, 30 | FloatVectorProperty, 31 | EnumProperty, 32 | PointerProperty, 33 | CollectionProperty 34 | ) 35 | from bpy.types import (Panel, 36 | Menu, 37 | Operator, 38 | PropertyGroup, 39 | UIList 40 | ) 41 | 42 | list_of_links = [] 43 | 44 | # ------------------------------------------------------------------------ 45 | # Structures 46 | # ------------------------------------------------------------------------ 47 | 48 | class rcb_wrapper(): 49 | def __init__(self, driver, icm, iposDir, ipos, ienc, encs, iax, joint_limits): 50 | self.driver = driver 51 | self.icm = icm 52 | self.iposDir = iposDir 53 | self.ipos = ipos 54 | self.ienc = ienc 55 | self.encs = encs 56 | self.iax = iax 57 | self.joint_limits = joint_limits 58 | 59 | 60 | # ------------------------------------------------------------------------ 61 | # Operators 62 | # ------------------------------------------------------------------------ 63 | def register_rcb(rcb_instance, rcb_name): 64 | scene = bpy.types.Scene 65 | scene.rcb_wrapper[rcb_name] = rcb_instance 66 | 67 | 68 | def unregister_rcb(rcb_name): 69 | try: 70 | del bpy.types.Scene.rcb_wrapper[rcb_name] 71 | except: 72 | pass 73 | 74 | 75 | def move(dummy): 76 | threshold = 10.0 # degrees 77 | scene = bpy.types.Scene 78 | mytool = bpy.context.scene.my_tool 79 | for key in scene.rcb_wrapper: 80 | rcb_instance = scene.rcb_wrapper[key] 81 | # Get the handles 82 | icm = rcb_instance.icm 83 | iposDir = rcb_instance.iposDir 84 | ipos = rcb_instance.ipos 85 | ienc = rcb_instance.ienc 86 | encs = rcb_instance.encs 87 | iax = rcb_instance.iax 88 | joint_limits = rcb_instance.joint_limits 89 | # Get the targets from the rig 90 | ok_enc = ienc.getEncoders(encs.data()) 91 | if not ok_enc: 92 | print("I cannot read the encoders, skipping") 93 | return 94 | for joint in range(0, ipos.getAxes()): 95 | # TODO handle the name of the armature, just keep iCub for now 96 | joint_name = iax.getAxisName(joint) 97 | if joint_name not in bpy.data.objects[mytool.my_armature].pose.bones.keys(): 98 | continue 99 | 100 | target = math.degrees(bpy.data.objects[mytool.my_armature].pose.bones[joint_name].rotation_euler[1]) 101 | min = joint_limits[joint][0] 102 | max = joint_limits[joint][1] 103 | if target < min or target > max: 104 | print("The target", target, "it is outside the boundaries (", min, ",", max, "), skipping.") 105 | continue 106 | 107 | safety_check=None 108 | # The icub hands encoders are not reliable for the safety check. 109 | if mytool.my_armature == "iCub" and joint > 5 : 110 | safety_check = False 111 | else: 112 | safety_check = (abs(encs[joint] - target) > threshold) 113 | 114 | if safety_check: 115 | print("The target is too far, reaching in position control, for joint", joint_name, "by ", abs(encs[joint] - target), " degrees" ) 116 | 117 | # Pause the animation 118 | bpy.ops.screen.animation_play() # We have to check if it is ok 119 | # Switch to position control and move to the target 120 | # TODO try to find a way to use the s methods 121 | icm.setControlMode(joint, yarp.VOCAB_CM_POSITION) 122 | ipos.setRefSpeed(joint,10) 123 | ipos.positionMove(joint,target) 124 | done = ipos.isMotionDone(joint) 125 | while not done: 126 | done = ipos.isMotionDone(joint) 127 | yarp.delay(0.001) 128 | # Once finished put the joints in position direct and replay the animation back 129 | icm.setControlMode(joint, yarp.VOCAB_CM_POSITION_DIRECT) 130 | bpy.ops.screen.animation_play() 131 | else: 132 | iposDir.setPosition(joint,target) 133 | 134 | 135 | def float_callback(self, context): 136 | # Callback for sliders. Find each object in the links dictionary and set its rotation. 137 | try: 138 | joint_tool = context.scene.my_joints 139 | pose_bones = bpy.data.objects[bpy.context.scene.my_tool.my_armature].pose.bones 140 | for joint_name, joint_value in joint_tool.items(): 141 | joint = pose_bones[joint_name] 142 | # It is a prismatic joint (to be tested) 143 | if joint.lock_rotation[1]: 144 | joint.delta_location[1] = joint_value 145 | # It is a revolute joint 146 | else: 147 | joint.rotation_euler[1] = joint_value * math.pi / 180.0 148 | joint.keyframe_insert(data_path="rotation_euler") 149 | 150 | except AttributeError: 151 | pass 152 | 153 | 154 | class AllJoints: 155 | 156 | def __init__(self): 157 | self.annotations = {} 158 | self.joint_names = [] 159 | self.generate_joint_classes() 160 | 161 | def generate_joint_classes(self): 162 | 163 | self.joint_names = bpy.data.objects[bpy.context.scene.my_tool.my_armature].pose.bones.keys() 164 | 165 | for joint_name, joint in bpy.data.objects[bpy.context.scene.my_tool.my_armature].pose.bones.items(): 166 | 167 | # Our bones rotate around y (revolute joint), translate along y (prismatic joint), if both are locked, it 168 | # means it is a fixed joint. 169 | if joint.lock_rotation[1] and joint.lock_location[1]: 170 | continue 171 | 172 | joint_min = -360 173 | joint_max = 360 174 | 175 | rot_constraint = None 176 | for constraint in joint.constraints: 177 | if constraint.type == "LIMIT_ROTATION": 178 | rot_constraint = constraint 179 | break 180 | if rot_constraint is not None: 181 | joint_min = rot_constraint.min_y * 180 / math.pi 182 | joint_max = rot_constraint.max_y * 180 / math.pi 183 | 184 | self.annotations[joint_name] = FloatProperty( 185 | name=joint_name, 186 | description=joint_name, 187 | default=0, 188 | min=joint_min, 189 | max=joint_max, 190 | update=float_callback, 191 | ) 192 | 193 | 194 | # ------------------------------------------------------------------------ 195 | # Scene Properties 196 | # ------------------------------------------------------------------------ 197 | 198 | def getLinks(self, context): 199 | return list_of_links 200 | 201 | class MyProperties(PropertyGroup): 202 | 203 | my_bool: BoolProperty( 204 | name="Dry run", 205 | description="If ticked, the movement will not replayed", 206 | default=False 207 | ) 208 | 209 | my_int: IntProperty( 210 | name="Int Value", 211 | description="A integer property", 212 | default=23, 213 | min=10, 214 | max=100 215 | ) 216 | 217 | my_float: FloatProperty( 218 | name="Threshold(degrees)", 219 | description="Threshold for the safety checks", 220 | default=5.0, 221 | min=2.0, 222 | max=15.0 223 | ) 224 | 225 | my_float_vector: FloatVectorProperty( 226 | name="Float Vector Value", 227 | description="Something", 228 | default=(0.0, 0.0, 0.0), 229 | min=0.0, 230 | max=0.1 231 | ) 232 | 233 | my_string: StringProperty( 234 | name="Robot", 235 | description=":", 236 | default="icub", 237 | maxlen=1024, 238 | ) 239 | 240 | my_armature: StringProperty( 241 | name="Armature name", 242 | description=":", 243 | default="iCub", 244 | maxlen=1024, 245 | ) 246 | 247 | my_path: StringProperty( 248 | name="Directory", 249 | description="Choose a directory:", 250 | default="", 251 | maxlen=1024, 252 | subtype='DIR_PATH' 253 | ) 254 | 255 | my_reach_x: FloatProperty( 256 | name="X", 257 | description="The target along x axis", 258 | default=0.0, 259 | min = -100.0, 260 | max = 100.0 261 | ) 262 | 263 | my_reach_y: FloatProperty( 264 | name="Y", 265 | description="The target along y axis", 266 | default=0.0, 267 | min=-100.0, 268 | max=100.0 269 | ) 270 | 271 | my_reach_z: FloatProperty( 272 | name="Z", 273 | description="The target along z axis", 274 | default=0.0, 275 | min=-100.0, 276 | max=100.0 277 | ) 278 | 279 | my_reach_pitch: FloatProperty( 280 | name="Pitch", 281 | description="The target around Pitch", 282 | default=0.0, 283 | min=-360.0, 284 | max=360.0 285 | ) 286 | 287 | my_reach_yaw: FloatProperty( 288 | name="Yaw", 289 | description="The target around Yaw", 290 | default=0.0, 291 | min=-360.0, 292 | max=360.0 293 | ) 294 | 295 | my_reach_roll: FloatProperty( 296 | name="Roll", 297 | description="The target around Roll", 298 | default=0.0, 299 | min=-360.0, 300 | max=360.0 301 | ) 302 | 303 | my_baseframeenum: EnumProperty( 304 | name="Base frame name:", 305 | description="Select the base frame:", 306 | items=getLinks 307 | ) 308 | my_eeframeenum: EnumProperty( 309 | name="End-effector frame name:", 310 | description="Select the end-effector frame:", 311 | items=getLinks 312 | ) 313 | 314 | 315 | class ListItem(PropertyGroup): 316 | value: StringProperty( 317 | name="Name", 318 | description="A name for this item", 319 | default="Untitled") 320 | 321 | viewValue: StringProperty( 322 | name="Displayed Name", 323 | description="", 324 | default="") 325 | 326 | isConnected: BoolProperty( 327 | name="", 328 | default = False 329 | ) 330 | 331 | 332 | class MY_UL_List(UIList): 333 | 334 | def draw_item(self, context, layout, data, item, icon, active_data, 335 | active_propname, index): 336 | if (item.isConnected): 337 | custom_icon = 'LINKED' 338 | else: 339 | custom_icon = 'UNLINKED' 340 | 341 | if self.layout_type in {'DEFAULT', 'COMPACT'}: 342 | layout.label(text=item.viewValue, icon = custom_icon) 343 | elif self.layout_type in {'GRID'}: 344 | layout.alignment = 'CENTER' 345 | layout.label(text=item.viewValue, icon = custom_icon) 346 | 347 | 348 | class WM_OT_Disconnect(bpy.types.Operator): 349 | bl_label = "Disconnect" 350 | bl_idname = "wm.disconnect" 351 | bl_description= "disconnect the selected part(s)" 352 | 353 | def execute(self, context): 354 | scene = bpy.context.scene 355 | parts = scene.my_list 356 | rcb_instance = bpy.types.Scene.rcb_wrapper[getattr(parts[scene.list_index], "value")] 357 | 358 | if rcb_instance is None: 359 | return {'CANCELLED'} 360 | rcb_instance.driver.close() 361 | 362 | del bpy.types.Scene.rcb_wrapper[getattr(parts[scene.list_index], "value")] 363 | 364 | setattr(parts[scene.list_index], "isConnected", False) 365 | 366 | return {'FINISHED'} 367 | 368 | 369 | class WM_OT_Connect(bpy.types.Operator): 370 | bl_label = "Connect" 371 | bl_idname = "wm.connect" 372 | bl_description= "connect the selected part(s)" 373 | 374 | def execute(self, context): 375 | scene = bpy.context.scene 376 | parts = scene.my_list 377 | mytool = scene.my_tool 378 | 379 | yarp.Network.init() 380 | if not yarp.Network.checkNetwork(): 381 | printError(self, "YARP server is not running!") 382 | return {'CANCELLED'} 383 | 384 | options = yarp.Property() 385 | driver = yarp.PolyDriver() 386 | 387 | # set the poly driver options 388 | options.put("robot", mytool.my_string) 389 | options.put("device", "remote_controlboard") 390 | options.put("local", "/blender_controller/client/"+getattr(parts[scene.list_index], "value")) 391 | options.put("remote", "/"+mytool.my_string+"/"+getattr(parts[scene.list_index], "value")) 392 | 393 | # opening the drivers 394 | print('Opening the motor driver...') 395 | driver.open(options) 396 | 397 | if not driver.isValid(): 398 | printError(self, "Cannot open the driver!") 399 | return {'CANCELLED'} 400 | 401 | # opening the drivers 402 | print('Viewing motor position/encoders...') 403 | icm = driver.viewIControlMode() 404 | iposDir = driver.viewIPositionDirect() 405 | ipos = driver.viewIPositionControl() 406 | ienc = driver.viewIEncoders() 407 | iax = driver.viewIAxisInfo() 408 | ilim = driver.viewIControlLimits() 409 | if ienc is None or ipos is None or icm is None or iposDir is None or iax is None or ilim is None: 410 | printError(self, "Cannot view one of the interfaces!") 411 | return {'CANCELLED'} 412 | 413 | encs = yarp.Vector(ipos.getAxes()) 414 | joint_limits = [] 415 | 416 | for joint in range(0, ipos.getAxes()): 417 | min = yarp.Vector(1) 418 | max = yarp.Vector(1) 419 | icm.setControlMode(joint, yarp.VOCAB_CM_POSITION_DIRECT) 420 | ilim.getLimits(joint, min.data(), max.data()) 421 | joint_limits.append([min.get(0), max.get(0)]) 422 | 423 | register_rcb(rcb_wrapper(driver, icm, iposDir, ipos, ienc, encs, iax, joint_limits), getattr(parts[scene.list_index], "value")) 424 | 425 | setattr(parts[scene.list_index], "isConnected", True) 426 | 427 | # TODO check if we need this 428 | #bpy.app.handlers.frame_change_post.clear() 429 | #bpy.app.handlers.frame_change_post.append(move) 430 | 431 | return {'FINISHED'} 432 | 433 | 434 | class WM_OT_Configure(bpy.types.Operator): 435 | bl_label = "Configure" 436 | bl_idname = "wm.configure" 437 | bl_description= "configure the parts by uploading a configuration file (.json format)" 438 | 439 | def execute(self, context): 440 | scene = bpy.context.scene 441 | mytool = scene.my_tool 442 | 443 | bpy.ops.rcb_panel.open_filebrowser('INVOKE_DEFAULT') 444 | 445 | try: 446 | # init the callback 447 | bpy.app.handlers.frame_change_post.append(move) 448 | except: 449 | printError(self, "A problem when initialising the callback") 450 | 451 | robot = AllJoints() 452 | 453 | # Dynamically create the same class 454 | JointProperties = type( 455 | # Class name 456 | "JointProperties", 457 | 458 | # Base class 459 | (bpy.types.PropertyGroup, ), 460 | {"__annotations__": robot.annotations}, 461 | ) 462 | 463 | # OBJECT_PT_robot_controller.set_joint_names(my_list) 464 | bpy.utils.register_class(JointProperties) 465 | bpy.types.Scene.my_joints = PointerProperty(type=JointProperties) 466 | 467 | return {'FINISHED'} 468 | 469 | 470 | class WM_OT_ReachTarget(bpy.types.Operator): 471 | bl_label = "Reach Target" 472 | bl_idname = "wm.reach_target" 473 | 474 | bl_description = "Reach the cartesian target" 475 | 476 | def execute(self, context): 477 | return InverseKinematics.execute(self) 478 | 479 | 480 | class WM_OT_initiate_drag_drop(bpy.types.Operator): 481 | """Process input while Control key is pressed""" 482 | bl_idname = 'wm.initiate_drag_drop' 483 | bl_label = 'Drag & Drop' 484 | # bl_options = {'REGISTER'} 485 | 486 | def execute(self, context): 487 | print("Going to invoke the modal operator") 488 | bpy.ops.object.modal_operator('INVOKE_DEFAULT') 489 | return {'FINISHED'} 490 | 491 | class ModalOperator(bpy.types.Operator): 492 | bl_idname = "object.modal_operator" 493 | bl_label = "Drap and Drop Operator" 494 | 495 | def __init__(self): 496 | print("Invoked!!!") 497 | self.mouse_pos = [0.0, 0.0] 498 | self.object = None 499 | self.loc_3d = [0.0, 0.0, 0.0] 500 | 501 | def __del__(self): 502 | print("End operator") 503 | 504 | def execute(self, context): 505 | print("location: ", self.loc_3d[0], self.loc_3d[1], self.loc_3d[2]) 506 | return {'FINISHED'} 507 | 508 | def modal(self, context, event): 509 | if event.type == 'LEFTMOUSE': # Apply 510 | self.loc_3d = [event.mouse_region_x, event.mouse_region_y] 511 | 512 | self.object = bpy.context.object 513 | region = bpy.context.region 514 | region3D = bpy.context.space_data.region_3d 515 | #The direction indicated by the mouse position from the current view 516 | view_vector = view3d_utils.region_2d_to_vector_3d(region, region3D, self.loc_3d) 517 | #The 3D location in this direction 518 | self.loc_3d = view3d_utils.region_2d_to_location_3d(region, region3D, self.loc_3d, view_vector) 519 | #The 3D location converted in object local coordinates 520 | # self.loc_3d = self.object.matrix_world.inverted() * mouse_loc 521 | 522 | InverseKinematics.execute(self, self.loc_3d) 523 | 524 | self.execute(context) 525 | 526 | elif event.type == 'RIGHTMOUSE': # Cancel 527 | print("Quit pressed") 528 | return {'CANCELLED'} 529 | 530 | return {'RUNNING_MODAL'} 531 | 532 | def invoke(self, context, event): 533 | if context.area.type == 'VIEW_3D': 534 | print("Drag and Drop operator invoked") 535 | args = (self, context) 536 | # self._handle = bpy.types.SpaceView3D.draw_handler_add(draw_callback_px, args, 'WINDOW', 'POST_PIXEL') 537 | 538 | # Keeps mouse position current 3D location and current object for the draw callback 539 | # (not needed to make self attribute if you don't want to use the callback) 540 | self.loc_3d = [0, 0, 0] 541 | 542 | self.object = context.object 543 | 544 | context.window_manager.modal_handler_add(self) 545 | return {'RUNNING_MODAL'} 546 | else: 547 | self.report({'WARNING'}, "View3D not found, cannot run operator") 548 | return {'CANCELLED'} 549 | 550 | # ------------------------------------------------------------------------ 551 | # Panel in Object Mode 552 | # ------------------------------------------------------------------------ 553 | 554 | class OBJECT_PT_robot_controller(Panel): 555 | bl_label = "Robot controller" 556 | bl_idname = "OBJECT_PT_robot_controller" 557 | bl_space_type = "VIEW_3D" 558 | bl_region_type = "UI" 559 | bl_category = "Tools" 560 | bl_context = "posemode" 561 | # joint_name = [] 562 | 563 | # @classmethod 564 | # def poll(cls, context): 565 | # return context.object is not None 566 | 567 | # @staticmethod 568 | # def set_joint_names(joint_names): 569 | # OBJECT_PT_robot_controller.joint_names = joint_names 570 | 571 | def draw(self, context): 572 | 573 | if not bones_with_driver: 574 | look_for_bones_with_drivers(context.scene.my_tool.my_armature) 575 | 576 | if ikv.iDynTreeModel is None: 577 | configure_ik() 578 | 579 | layout = self.layout 580 | scene = context.scene 581 | parts = scene.my_list 582 | mytool = scene.my_tool 583 | rcb_wrapper = bpy.types.Scene.rcb_wrapper 584 | 585 | box_configure = layout.box() 586 | box_configure.prop(mytool, "my_armature") 587 | box_configure.operator("wm.configure") 588 | 589 | box = layout.box() 590 | box.label(text="Selection Tools") 591 | box.template_list("MY_UL_List", "The_List", scene, 592 | "my_list", scene, "list_index") 593 | 594 | box.prop(mytool, "my_string") 595 | row_connect = box.row(align=True) 596 | row_connect.operator("wm.connect") 597 | layout.separator() 598 | row_disconnect = box.row(align=True) 599 | row_disconnect.operator("wm.disconnect") 600 | layout.separator() 601 | 602 | reach_box = layout.box() 603 | reach_box.label(text="Reach target") 604 | reach_box.row(align=True).prop(mytool, "my_baseframeenum") 605 | reach_box.row(align=True).prop(mytool, "my_eeframeenum") 606 | 607 | reach_box.label(text="Position") 608 | reach_box.row(align=True).prop(mytool, "my_reach_x") 609 | reach_box.row(align=True).prop(mytool, "my_reach_y") 610 | reach_box.row(align=True).prop(mytool, "my_reach_z") 611 | 612 | reach_box.label(text="Rotation") 613 | reach_box.row(align=True).prop(mytool, "my_reach_roll") 614 | reach_box.row(align=True).prop(mytool, "my_reach_pitch") 615 | reach_box.row(align=True).prop(mytool, "my_reach_yaw") 616 | reach_box.operator("wm.reach_target") 617 | 618 | reach_box.operator("wm.initiate_drag_drop") 619 | 620 | layout.separator() 621 | 622 | box_joints = layout.box() 623 | box_joints.label(text="joint angles") 624 | 625 | try: 626 | scene.my_joints 627 | except AttributeError: 628 | joints_exist = False 629 | else: 630 | joints_exist = True 631 | for joint_name, joint in bpy.data.objects[mytool.my_armature].pose.bones.items(): 632 | # We do not have to add the entry in the list for the bones that have drivers 633 | # since they have not to be controlled directly, but throgh the driver.s 634 | if joint_name in bones_with_driver: 635 | continue 636 | # Our bones rotate around y (revolute joint), translate along y (prismatic joint), if both are locked, it 637 | # means it is a fixed joint. 638 | if joint.lock_rotation[1] and joint.lock_location[1]: 639 | continue 640 | box_joints.prop(scene.my_joints, joint_name) 641 | 642 | if len(context.scene.my_list) == 0 or not joints_exist: 643 | box.enabled = False 644 | box_configure.enabled = True 645 | box_joints.enabled = False 646 | reach_box.enabled = False 647 | else: 648 | box.enabled = True 649 | box_configure.enabled = False 650 | if bpy.context.screen.is_animation_playing: 651 | row_disconnect.enabled = False 652 | row_connect.enabled = False 653 | box_joints.enabled = False 654 | reach_box.enabled = False 655 | else: 656 | box_joints.enabled = True 657 | reach_box.enabled = ikv.configured 658 | if getattr(parts[scene.list_index], "value") in rcb_wrapper.keys(): 659 | row_disconnect.enabled = True 660 | row_connect.enabled = False 661 | else: 662 | row_disconnect.enabled = False 663 | row_connect.enabled = True 664 | 665 | 666 | class OT_OpenConfigurationFile(Operator, ImportHelper): 667 | 668 | bl_idname = "rcb_panel.open_filebrowser" 669 | bl_label = "Select the configuration file" 670 | 671 | filter_glob: StringProperty( 672 | default='*.json', 673 | options={'HIDDEN'} 674 | ) 675 | 676 | def parse_conf(self, filepath, context): 677 | f = open(filepath) 678 | data = json.load(f) 679 | context.scene.my_list.clear() 680 | 681 | for p in data['parts']: 682 | item = context.scene.my_list.add() 683 | item.value = p[0] 684 | item.viewValue = p[1] 685 | 686 | def execute(self, context): 687 | filename, extension = os.path.splitext(self.filepath) 688 | self.parse_conf(self.filepath, context) 689 | # bpy.ops.object.process_input('INVOKE_DEFAULT') 690 | return {'FINISHED'} 691 | 692 | 693 | def configure_ik(): 694 | if 'model_urdf' not in bpy.context.scene: 695 | ikv.configured = False 696 | return 697 | ikv.configured = True 698 | model_urdf = bpy.context.scene['model_urdf'] 699 | mdlLoader = iDynTree.ModelLoader() 700 | mdlLoader.loadModelFromString(model_urdf) 701 | ikv.iDynTreeModel = mdlLoader.model() 702 | 703 | # list_of_links = [] 704 | for link_idx in range(ikv.iDynTreeModel.getNrOfLinks()): 705 | list_of_links.append((ikv.iDynTreeModel.getLinkName(link_idx), 706 | ikv.iDynTreeModel.getLinkName(link_idx), 707 | "")) 708 | 709 | ikv.inverseKinematics.setModel(ikv.iDynTreeModel) 710 | # Setup the ik problem 711 | ikv.inverseKinematics.setCostTolerance(0.0001) 712 | ikv.inverseKinematics.setConstraintsTolerance(0.00001) 713 | ikv.inverseKinematics.setDefaultTargetResolutionMode(iDynTree.InverseKinematicsTreatTargetAsConstraintNone) 714 | ikv.inverseKinematics.setRotationParametrization(iDynTree.InverseKinematicsRotationParametrizationRollPitchYaw) 715 | 716 | ikv.dynComp.loadRobotModel(ikv.iDynTreeModel) 717 | dofs = ikv.dynComp.model().getNrOfDOFs() 718 | s = iDynTree.VectorDynSize(dofs) 719 | for dof in range(dofs): 720 | s.setVal(dof, 0.0) 721 | ikv.dynComp.setJointPos(s) 722 | -------------------------------------------------------------------------------- /script/blenderRCBPanel/common_functions.py: -------------------------------------------------------------------------------- 1 | import bpy 2 | import math 3 | import idyntree.bindings as iDynTree 4 | 5 | 6 | class IkVariables: 7 | inverseKinematics = iDynTree.InverseKinematics() 8 | dynComp = iDynTree.KinDynComputations() 9 | iDynTreeModel = None 10 | configured = False 11 | 12 | bones_with_driver = [] 13 | 14 | 15 | def printError(object, *args): 16 | object.report({"ERROR"}, " ".join(args)) 17 | 18 | def look_for_bones_with_drivers(armature_name): 19 | for joint_name in bpy.data.objects[armature_name].pose.bones.keys(): 20 | # It means that there are no drivers 21 | if bpy.context.object.animation_data is None: 22 | return 23 | for d in bpy.context.object.animation_data.drivers: 24 | if ('"%s"' % joint_name) in d.data_path: 25 | bones_with_driver.append(joint_name) 26 | 27 | class InverseKinematics: 28 | 29 | def __init__(self): 30 | pass 31 | 32 | @staticmethod 33 | def execute(object, xyz=[], rpy=[]): 34 | scene = bpy.context.scene 35 | considered_joints = [] 36 | ik = IkVariables.inverseKinematics 37 | model = ik.fullModel() 38 | 39 | # bpy.ops.object.wm_ot_reachTarget 40 | mytool = scene.my_tool 41 | 42 | base_frame = mytool.my_baseframeenum 43 | endeffector_frame = mytool.my_eeframeenum 44 | 45 | if base_frame == endeffector_frame: 46 | printError(object, "Base frame and end-effector frame are coincident!") 47 | return {'CANCELLED'} 48 | 49 | # TODO substitute the traversal part with this block after 50 | # DHChain has been added to the bindings 51 | # dhChain = iDynTree.DHChain() 52 | # dhChain.fromModel(model, base_frame, endeffector_frame) 53 | 54 | # for chain_idx in range(dhChain.getNrOfDOFs()): 55 | # considered_joints.append(dhChain.getDOFName(chain_idx)) 56 | 57 | traversal = iDynTree.Traversal() 58 | ok_traversal = model.computeFullTreeTraversal(traversal) 59 | 60 | if not ok_traversal: 61 | printError(object, "Unable to get the traversal") 62 | return {'CANCELLED'} 63 | 64 | base_link_idx = model.getLinkIndex(base_frame) 65 | endeffector_link_idx = model.getLinkIndex(endeffector_frame) 66 | if base_link_idx < 0 or endeffector_link_idx < 0: 67 | return {'CANCELLED'} 68 | 69 | visitedLinkIdx = endeffector_link_idx 70 | # create the list of considered joints, it is the list of the joints of 71 | # the selected chain 72 | while visitedLinkIdx != base_link_idx: 73 | parentLink = traversal.getParentLinkFromLinkIndex(visitedLinkIdx) 74 | if parentLink is None: 75 | printError(object, "Unable to find a single chain that goes from", base_frame, "to", endeffector_frame) 76 | return {'CANCELLED'} 77 | parentLinkIdx = parentLink.getIndex() 78 | joint = traversal.getParentJointFromLinkIndex(visitedLinkIdx) 79 | visitedLinkIdx = parentLinkIdx 80 | if joint.getNrOfDOFs() == 0: 81 | continue 82 | considered_joints.append(model.getJointName(joint.getIndex())) 83 | 84 | # Extract reduced model 85 | 86 | ik.setModel(model, considered_joints) 87 | IkVariables.dynComp.loadRobotModel(ik.reducedModel()) 88 | joint_positions = iDynTree.VectorDynSize(ik.reducedModel().getNrOfDOFs()) 89 | 90 | # Note: the InverseKinematics class actually implements a floating base inverse kinematics, 91 | # meaning that both the joint position and the robot base are optimized to reach the desired cartesian position 92 | world_H_base = IkVariables.dynComp.getWorldTransform(base_frame) 93 | 94 | ok = IkVariables.inverseKinematics.setFloatingBaseOnFrameNamed(base_frame) 95 | 96 | ik.addFrameConstraint(base_frame, world_H_base) 97 | 98 | # base_H_ee_initial = IkVariables.dynComp.getRelativeTransform(base_frame, endeffector_frame); 99 | 100 | if not rpy: 101 | iDynTreeRotation = iDynTree.Rotation.RPY(mytool.my_reach_roll * math.pi / 180, 102 | mytool.my_reach_pitch * math.pi / 180, 103 | mytool.my_reach_yaw * math.pi / 180) 104 | else: 105 | iDynTreeRotation = iDynTree.Rotation.RPY(rpy[0] * math.pi / 180, 106 | rpy[1] * math.pi / 180, 107 | rpy[2] * math.pi / 180) 108 | 109 | if not xyz: 110 | iDynTreePosition = iDynTree.Position(mytool.my_reach_x, mytool.my_reach_y, mytool.my_reach_z) 111 | else: 112 | iDynTreePosition = iDynTree.Position(-xyz[0], xyz[1], xyz[2]) 113 | 114 | # Define the transform of the selected cartesian target 115 | base_H_ee_desired = iDynTree.Transform(iDynTreeRotation, iDynTreePosition) 116 | 117 | # We want that the end effector reaches the target 118 | ok = ik.addTarget(endeffector_frame, base_H_ee_desired) 119 | if not ok: 120 | ok = ik.updateTarget(endeffector_frame, base_H_ee_desired) 121 | if not ok: 122 | printError(object, "Impossible to add target on ", endeffector_frame) 123 | return {'CANCELLED'} 124 | # Initialize ik 125 | IkVariables.dynComp.getJointPos(joint_positions) 126 | ik.setReducedInitialCondition(world_H_base, joint_positions) 127 | # ik.setDesiredReducedJointConfiguration(joint_positions) 128 | 129 | # TODO: The following line generated the WARNING: 130 | # [WARNING] InverseKinematics: joint l_elbow (index 20) 131 | # initial condition is outside the limits 0.261799 1.85005. Actual value: 0 132 | # [WARNING] InverseKinematics: joint r_elbow (index 28) 133 | # initial condition is outside the limits 0.261799 1.85005. Actual value: 0 134 | ok = ik.solve() 135 | 136 | if not ok: 137 | printError(object, "Impossible to solve inverse kinematics problem.") 138 | return {'CANCELLED'} 139 | 140 | base_transform = iDynTree.Transform.Identity() 141 | 142 | # Get the solution 143 | ik.getReducedSolution(base_transform, joint_positions) 144 | IkVariables.dynComp.setJointPos(joint_positions) 145 | 146 | pose_bones = bpy.data.objects[bpy.context.scene.my_tool.my_armature].pose.bones 147 | for idyn_joint_idx in range(ik.reducedModel().getNrOfDOFs()): 148 | 149 | # It is a prismatic joint (to be tested) 150 | joint_name = ik.reducedModel().getJointName(idyn_joint_idx) 151 | 152 | joint = pose_bones[joint_name] 153 | joint_value = joint_positions[idyn_joint_idx] 154 | 155 | if joint.lock_rotation[1]: 156 | joint.delta_location[1] = joint_value 157 | # It is a revolute joint 158 | else: 159 | joint.rotation_euler[1] = joint_value 160 | joint.keyframe_insert(data_path="rotation_euler") 161 | 162 | return {'FINISHED'} 163 | -------------------------------------------------------------------------------- /script/conf/parts.json: -------------------------------------------------------------------------------- 1 | { 2 | "parts": [ 3 | ["torso", "Torso"], 4 | ["head", "Head"], 5 | ["left_arm", "Left arm"], 6 | ["right_arm", "Right arm"], 7 | ["left_leg", "Left leg"], 8 | ["right_leg", "Right leg"] 9 | ] 10 | } 11 | -------------------------------------------------------------------------------- /script/urdfToBlender/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2022 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | bl_info = { 8 | "name": "URDF to Blender Panel", 9 | "description": "Panel converting urdf to .blend", 10 | "author": "Nicogene", 11 | "version": (0, 0, 1), 12 | "blender": (2, 80, 0), 13 | "location": "3D View > Tools", 14 | "warning": "", # used for warning icon and text in addons panel 15 | "wiki_url": "", 16 | "tracker_url": "", 17 | "category": "Development" 18 | } 19 | 20 | import bpy 21 | 22 | from bpy.props import (StringProperty, 23 | BoolProperty, 24 | IntProperty, 25 | FloatProperty, 26 | FloatVectorProperty, 27 | EnumProperty, 28 | PointerProperty, 29 | CollectionProperty 30 | ) 31 | 32 | from .urdfToBlender import (OBJECT_PT_urdf2blender_converter, 33 | WM_OT_OpenFilebrowser) 34 | 35 | # ------------------------------------------------------------------------ 36 | # Registration 37 | # ------------------------------------------------------------------------ 38 | 39 | classes = ( 40 | WM_OT_OpenFilebrowser, 41 | OBJECT_PT_urdf2blender_converter 42 | ) 43 | 44 | 45 | def register(): 46 | for cls in classes: 47 | try: 48 | bpy.utils.register_class(cls) 49 | except: 50 | print("an exception when registering the class") 51 | 52 | def unregister(): 53 | for cls in reversed(classes): 54 | try: 55 | bpy.utils.unregister_class(cls) 56 | except: 57 | print("An exception was raised when unregistering the class") 58 | 59 | 60 | if __name__ == "__main__": 61 | register() 62 | -------------------------------------------------------------------------------- /script/urdfToBlender/urdfToBlender.py: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT) 2 | # All rights reserved. 3 | # 4 | # This software may be modified and distributed under the terms of the 5 | # BSD-3-Clause license. See the accompanying LICENSE file for details. 6 | 7 | import bpy, bmesh 8 | import copy 9 | import mathutils 10 | import math 11 | import os 12 | import sys 13 | import idyntree.bindings as iDynTree 14 | import xml.etree.ElementTree as ET 15 | 16 | from bpy.props import StringProperty, BoolProperty 17 | from bpy_extras.io_utils import ImportHelper 18 | from bpy.types import (Panel, 19 | Menu, 20 | Operator, 21 | PropertyGroup, 22 | ) 23 | 24 | def createGeometricShape(iDynTree_solidshape): 25 | if iDynTree_solidshape.isSphere(): 26 | bpy.ops.mesh.primitive_uv_sphere_add(radius=iDynTree_solidshape.asSphere().getRadius()) 27 | elif iDynTree_solidshape.isCylinder(): 28 | cylinder = iDynTree_solidshape.asCylinder() 29 | bpy.ops.mesh.primitive_cylinder_add(radius=cylinder.getRadius(), depth=cylinder.getLength()) 30 | elif iDynTree_solidshape.isBox(): 31 | box = iDynTree_solidshape.asBox() 32 | if box.getX() != box.getY() or box.getZ() != box.getX() or box.getZ() != box.getY(): 33 | print("WARNING: the box has different dimensions along the three axis but it will be imported as cube with size equal to the X length") 34 | bpy.ops.mesh.primitive_cube_add(size=box.getX()) # Seems that blender support only cubes 35 | else: 36 | print("Geometric shape not supported") 37 | return False 38 | return True 39 | 40 | def rigify(path): 41 | 42 | armature_name = "" 43 | 44 | # Get robot name needed until https://github.com/robotology/idyntree/issues/908 is not fixed 45 | root = ET.parse(path).getroot() 46 | armature_name = root.attrib["name"] 47 | # Get the urdf and parse it 48 | dynComp = iDynTree.KinDynComputations(); 49 | mdlLoader = iDynTree.ModelLoader(); 50 | mdlExporter = iDynTree.ModelExporter(); 51 | urdf_str = "" 52 | with open(path, 'r') as file: 53 | urdf_str = file.read() 54 | mdlLoader.loadModelFromFile(path); 55 | 56 | # Produce the reduced urdf 57 | model = mdlLoader.model() 58 | 59 | # Save the model in the scene 60 | bpy.context.scene['model_urdf'] = urdf_str 61 | traversal = iDynTree.Traversal() 62 | ok_traversal = model.computeFullTreeTraversal(traversal) 63 | print(ok_traversal) 64 | if not ok_traversal: 65 | print("Failed to compute the traversal!") 66 | return 0 67 | linkVisual=model.visualSolidShapes().getLinkSolidShapes(); 68 | 69 | dofs = dynComp.model().getNrOfDOFs(); 70 | s = iDynTree.VectorDynSize(dofs); 71 | ds = iDynTree.VectorDynSize(dofs); 72 | dds = iDynTree.VectorDynSize(dofs); 73 | for dof in range(dofs): 74 | # For the sake of the example, we fill the joints vector with gibberish data (remember in any case 75 | # that all quantities are expressed in radians-based units 76 | s.setVal(dof, 0.0); 77 | ds.setVal(dof, 0.0); 78 | dds.setVal(dof, 0.3); 79 | 80 | 81 | # The gravity acceleration is a 3d acceleration vector. 82 | gravity = iDynTree.Vector3(); 83 | gravity.zero(); 84 | gravity.setVal(2, -9.81); 85 | dynComp.setRobotState(s,ds,gravity); 86 | 87 | dynComp.loadRobotModel(mdlLoader.model()); 88 | print("The loaded model has", dynComp.model().getNrOfDOFs(), \ 89 | "internal degrees of freedom and",dynComp.model().getNrOfLinks(),"links.") 90 | 91 | # Remove meshes leftovers 92 | # Will collect meshes from delete objects 93 | meshes = set() 94 | # Delete all the objects in the scene 95 | # Get objects in the collection if they are meshes 96 | for obj in bpy.data.objects: 97 | # Store the internal mesh 98 | if obj.type == 'MESH': 99 | meshes.add( obj.data ) 100 | # Delete the object 101 | bpy.data.objects.remove( obj ) 102 | # Look at meshes that are orphean after objects removal 103 | for mesh in [m for m in meshes if m.users == 0]: 104 | # Delete the meshes 105 | bpy.data.meshes.remove( mesh ) 106 | 107 | # Check if there are still orphean meshes 108 | # It may happens when you delete the object from UI 109 | for mesh in bpy.data.meshes: 110 | if mesh.name not in bpy.data.objects.keys(): 111 | bpy.data.meshes.remove( mesh ) 112 | 113 | # Import the meshes 114 | meshMap = {} 115 | meshesInfo = {} 116 | 117 | # import meshes and do the mapping to the link 118 | for link_id in range(model.getNrOfLinks()): 119 | if len(linkVisual[link_id]) == 0: 120 | continue 121 | meshesInfo[model.getLinkName(link_id)] = linkVisual[link_id][0] 122 | linkname = model.getLinkName(link_id) 123 | if meshesInfo[model.getLinkName(link_id)].isExternalMesh(): 124 | # import the mesh 125 | filePath = meshesInfo[model.getLinkName(link_id)].asExternalMesh().getFileLocationOnLocalFileSystem() 126 | if ".stl" in filePath: 127 | bpy.ops.import_mesh.stl(filepath=os.path.join(filePath),global_scale=0.001) 128 | elif ".ply" in filePath: 129 | bpy.ops.import_mesh.ply(filepath=os.path.join(filePath),global_scale=0.001) 130 | elif ".dae" in filePath: 131 | bpy.ops.wm.collada_import(filepath=os.path.join(filePath), import_units=True) #TODO check how to handle scale here ! 132 | else: 133 | # it is a basic geometry(sphere, cylinder, box) 134 | if not createGeometricShape(meshesInfo[model.getLinkName(link_id)]): 135 | continue 136 | meshName = "" 137 | # We are assuming we are starting in a clean environment 138 | if not meshMap.keys() : 139 | meshName = bpy.data.objects.keys()[0] 140 | else: 141 | for mesh in bpy.data.objects: 142 | if mesh.name not in meshMap.values(): 143 | meshName = mesh.name 144 | break 145 | meshMap[linkname] = meshName 146 | 147 | # Place the meshes 148 | for link_id in range(model.getNrOfLinks()): 149 | linkname = model.getLinkName(link_id) 150 | if linkname not in meshMap.keys(): 151 | continue 152 | meshname = meshMap[linkname] 153 | meshobj = bpy.data.objects[meshname] 154 | # root->link transform 155 | RtoLinktransform = dynComp.getRelativeTransform("root_link", linkname) 156 | # link->geometry transform 157 | LinkToGtransform = meshesInfo[linkname].getLink_H_geometry() 158 | # root->geometry transform 159 | RToGtransform = RtoLinktransform * LinkToGtransform 160 | 161 | meshobj.location = RToGtransform.getPosition().toNumPy() 162 | meshobj.rotation_mode = "QUATERNION" 163 | meshobj.rotation_quaternion = RToGtransform.getRotation().asQuaternion() 164 | 165 | # Define the armature 166 | # Create armature and armature object 167 | try: 168 | armature_object = bpy.data.objects[armature_name] 169 | except KeyError: 170 | # create armature 171 | armature = bpy.data.armatures.new(armature_name) 172 | armature_object = bpy.data.objects.new(armature_name, armature) 173 | # Link armature object to our scene 174 | bpy.context.scene.collection.objects.link(armature_object) 175 | 176 | #Make a coding shortcut 177 | armature_data = bpy.data.objects[armature_name] 178 | # must be in edit mode to add bones 179 | bpy.context.view_layer.objects.active = armature_object 180 | bpy.ops.object.mode_set(mode='EDIT') 181 | edit_bones = armature_data.data.edit_bones 182 | pose_bones = armature_data.pose.bones 183 | 184 | limits = {} 185 | bone_list = {} 186 | # Loop for defining the hierarchy of the bonse and its locations 187 | for idyn_joint_idx in range(model.getNrOfJoints()): 188 | parentIdx = traversal.getParentLinkIndexFromJointIndex(model, 189 | idyn_joint_idx) 190 | childIdx = traversal.getChildLinkIndexFromJointIndex(model, 191 | idyn_joint_idx) 192 | parentname = model.getLinkName(parentIdx) 193 | childname = model.getLinkName(childIdx) 194 | joint = model.getJoint(idyn_joint_idx) 195 | jointtype = "" 196 | if joint.isRevoluteJoint(): 197 | joint = joint.asRevoluteJoint() 198 | jointtype = "REVOLUTE" 199 | direction = joint.getAxis(childIdx,parentIdx).getDirection().toNumPy() 200 | # This is missing from idyntree api :( 201 | #elif joint.isPrismaticJoint(): 202 | # joint = joint.asPrismaticJoint() 203 | # jointtype = "PRISMATIC" 204 | elif joint.isFixedJoint(): 205 | joint = joint.asFixedJoint() 206 | jointtype = "FIXED" 207 | #else: 208 | # joint = joint.asRevoluteJoint() 209 | # jointtype = "REVOLUTE" 210 | # direction = joint.getAxis(childIdx,parentIdx).getDirection().toNumPy() 211 | min = joint.getMinPosLimit(0) 212 | max = joint.getMaxPosLimit(0) 213 | bparent = None 214 | if parentname in bone_list.keys(): 215 | bparent = bone_list[parentname] 216 | else: 217 | if parentname != "root_link": 218 | for i in range(model.getNrOfJoints()): 219 | childname_prev = model.getLinkName(traversal.getChildLinkIndexFromJointIndex(model, 220 | i)) 221 | if childname_prev == parentname: 222 | bonename = model.getJointName(i) 223 | if bonename not in edit_bones.keys(): 224 | bparent = edit_bones.new(bonename) 225 | else: 226 | bparent = edit_bones[bonename] 227 | break 228 | else: 229 | bparent = edit_bones.new(parentname) 230 | # TODO I have to put random value for head and tail bones otherwise bones with 0 lenght are removed 231 | bparent.head = (0,0,0) 232 | bparent.tail = (0,0,-0.01) 233 | bone_list[parentname] = bparent 234 | 235 | bonename = model.getJointName(idyn_joint_idx) 236 | if bonename not in edit_bones.keys(): 237 | bchild = edit_bones.new(bonename) 238 | else: 239 | bchild = edit_bones[bonename] 240 | 241 | if bparent: 242 | bchild.parent = bparent 243 | 244 | parent_link_transform = dynComp.getRelativeTransform("root_link", parentname) 245 | parent_link_position = parent_link_transform.getPosition().toNumPy(); 246 | child_link_transform = dynComp.getRelativeTransform("root_link", childname) 247 | child_link_position = child_link_transform.getPosition().toNumPy(); 248 | child_link_rotation = mathutils.Matrix(child_link_transform.getRotation().toNumPy()); 249 | # Start defining the bone like parent->child link 250 | bchild.head = parent_link_position 251 | bchild.tail = child_link_position 252 | if jointtype == "REVOLUTE": 253 | length = bchild.length 254 | if length == 0.0: 255 | length = 0.01 # bones with zero length are deleted by Blender 256 | direction = mathutils.Vector(direction).normalized() 257 | direction.rotate(child_link_rotation) 258 | # In our representation the revolute joint is a bone placed in the child origin 259 | # oriented towards the axis of the joint. 260 | bchild.head = child_link_position 261 | bchild.tail = bchild.head + direction * length 262 | 263 | bone_list[childname] = bchild 264 | # Consider the y-axis orientation in the limits 265 | limits[model.getJointName(idyn_joint_idx)] = [min, max, jointtype] 266 | 267 | # exit edit mode to save bones so they can be used in pose mode 268 | bpy.ops.object.mode_set(mode='OBJECT') 269 | # just for checking that the map link->mesh is ok. 270 | #for k,v in meshMap.items(): 271 | # print(k,v) 272 | 273 | # Now iterate over all the joints(bones) and link them to the meshes. 274 | for idyn_joint_idx in range(model.getNrOfJoints()): 275 | # The joint should move the child link(?) 276 | childIdx = traversal.getChildLinkIndexFromJointIndex(model, 277 | idyn_joint_idx) 278 | childname = model.getLinkName(childIdx) 279 | if childname not in meshMap.keys(): 280 | continue 281 | jointname = model.getJointName(idyn_joint_idx) 282 | meshname = meshMap[childname] 283 | meshobj = bpy.data.objects[meshname] 284 | 285 | bpy.ops.object.select_all(action='DESELECT') 286 | armature_data.select_set(True) 287 | bpy.context.view_layer.objects.active = armature_data 288 | 289 | bpy.ops.object.mode_set(mode='EDIT') 290 | 291 | edit_bones.active = edit_bones[jointname] 292 | 293 | bpy.ops.object.mode_set(mode='OBJECT') 294 | 295 | bpy.ops.object.select_all(action='DESELECT') #deselect all objects 296 | meshobj.select_set(True) 297 | armature_data.select_set(True) 298 | bpy.context.view_layer.objects.active = armature_data #the active object will be the parent of all selected object 299 | 300 | bpy.ops.object.parent_set(type='BONE', keep_transform=True) 301 | 302 | # configure the bones limits 303 | bpy.ops.object.mode_set(mode='POSE') 304 | for pbone in pose_bones: 305 | bone_name = pbone.basename 306 | pbone.lock_location = (True, True, True) 307 | pbone.lock_rotation = (True, True, True) 308 | pbone.lock_scale = (True, True, True) 309 | # root_link is a special case, it is a bone that has not correspondences to the joints 310 | if bone_name == "root_link": 311 | continue 312 | lim = limits[bone_name] 313 | # check the nr of DOFs 314 | if lim[2] == "FIXED" : 315 | continue 316 | 317 | c = pbone.constraints.new('LIMIT_ROTATION') 318 | c.owner_space = 'LOCAL' 319 | 320 | if lim[2] == "REVOLUTE": 321 | # The bones should rotate around y-axis 322 | pbone.lock_rotation[1] = False 323 | elif lim[2] == "PRISMATIC": 324 | # The bones should move along y-axis 325 | pbone.lock_location[1] = False 326 | if lim: 327 | c.use_limit_y = True 328 | # TODO maybe we have to put also the ik constraints ??? 329 | #print(bone_name, math.degrees(lim[0]), math.degrees(lim[1])) 330 | c.min_y = lim[0] # min 331 | c.max_y = lim[1] # max 332 | 333 | # TODO not sure if it is the right rotation_mode 334 | pbone.rotation_mode = 'XYZ' 335 | 336 | bpy.context.scene.transform_orientation_slots[0].type = 'LOCAL' 337 | 338 | 339 | class WM_OT_OpenFilebrowser(Operator, ImportHelper): 340 | 341 | bl_idname = "wm.open_filebrowser" 342 | bl_label = "Select the urdf" 343 | 344 | filter_glob: StringProperty( 345 | default='*.urdf', 346 | options={'HIDDEN'} 347 | ) 348 | 349 | def execute(self, context): 350 | """Do something with the selected file(s).""" 351 | 352 | filename, extension = os.path.splitext(self.filepath) 353 | 354 | print('Selected file:', self.filepath) 355 | print('File name:', filename) 356 | print('File extension:', extension) 357 | rigify(self.filepath) 358 | 359 | return {'FINISHED'} 360 | 361 | class OBJECT_PT_urdf2blender_converter(Panel): 362 | bl_label = "URDF to Blender converter" 363 | bl_idname = "OBJECT_PT_urdf2blender_converter" 364 | bl_space_type = "VIEW_3D" 365 | bl_region_type = "UI" 366 | bl_category = "Tools" 367 | bl_context = "objectmode" 368 | row_convert = None 369 | 370 | def draw(self, context): 371 | layout = self.layout 372 | scene = context.scene 373 | row_configure = layout.row(align=True) 374 | row_configure.operator("wm.open_filebrowser") 375 | 376 | 377 | # Main function 378 | def main(urdf_filename, blend_filename): 379 | rigify(urdf_filename) 380 | bpy.ops.wm.save_as_mainfile(filepath=blend_filename) 381 | 382 | # Execute main() 383 | if __name__=='__main__': 384 | argv = sys.argv 385 | try: 386 | urdf_filename = argv[argv.index("--urdf_filename") + 1] 387 | except ValueError: 388 | urdf_filename = None 389 | try: 390 | blend_filename = argv[argv.index("--blend_filename") + 1] 391 | except ValueError: 392 | blend_filename = "./robot.blend" 393 | main(urdf_filename, blend_filename) 394 | --------------------------------------------------------------------------------