How can I update base body transformation matrix during visualization using 'show' function?

To visualize the rigidbody tree model from urdf file, the 'show' function can be used. However, it supports to set the base body position (x,y,z) and yaw angle, only... I want to arbitrary set the base body position and orientation.

For a internal function in the robotics toolbox, there is a little comment to use 6-DOF xyz & roll, ptich, yaw, thereby obtaining whole base body transformation matrix. But, the strict support does not provide or activate them.

How can I set arbitrary base body transformation matrix of rigidbody tree? I have to simulate the base body trasnform almost real-time, therefore I don't want to add and remove body during simulation.

Answer 1

[Garmit Pant]

Hello Jonghyeok Kim,

By default, the “rigidBodyTree” object represents rigid body trees with a fixed base. This means that the base body of the rigid body tree has a fixed position and orientation in the world. You can simulate robots with 6 degrees of freedom (DOF) by attaching a “floating” type rigid body joint to the base body of the robots. Floating-base robots can freely translate and rotate in space with six degrees of freedom.

The following code snippet demonstrates how to attach a “floating” base to the “rigidBodyTree” objects and visualize them using the “show” function:

% Create a rigid body tree object with data format specified as "row" 
rbt = rigidBodyTree(DataFormat="row"); 

% Create a floating base body and assign it a floating joint 
floatingBaseBody = rigidBody("floatingBase"); 
floatingBaseBody.Joint = rigidBodyJoint("j1","floating"); 

% Add the floating base body to the rigid body tree 
addBody(rbt, floatingBaseBody, rbt.BaseName); 

% Load the ABB IRB 120 robot model and set its data format to "row" 
abbirb = loadrobot("abbIrb120", DataFormat="row"); 

% Set the base name of the rigid body tree to 'world' 
rbt.BaseName = 'world'; 

% Add the ABB IRB 120 robot as a subtree to the floating base body 
% ReplaceBase is set to false to keep the existing base of the ABB IRB 120 robot 
addSubtree(rbt, "floatingBase", abbirb, ReplaceBase=false); 

% Define the base orientation using ZYX Euler angles and convert to quaternion 
baseOrientation = eul2quat([0 pi/3 pi/3]); % ZYX Euler rotation order 

% Define the base position 
basePosition = [-1.1 0.2 0.3]; 

% Combine the base orientation, base position, and the home configuration of the ABB IRB 120 robot 
floatingRBTConfig = [baseOrientation, basePosition, homeConfiguration(abbirb)]; 

% Display the robot in the specified configuration 
show(rbt, floatingRBTConfig); 
axis equal; % Set axis scaling to equal 
title(["Robot Joint Configuration With Base", "at Desired Position and Orientation"]); % Add title to the plot 

For further understanding, kindly refer to the following MathWorks Documentation:

• Refer to the ‘Examples’ section to understand the details of the “floating” base to a “rigidBodyTree” object: https://www.mathworks.com/help/robotics/ref/rigidbodyjoint.html#mw_625b73a4-b5d8-4838-b9e5-e6162a3b219f
• Refer to this example to understand how to use robots with “floating” base and apply inverse kinematics to them: https://www.mathworks.com/help/robotics/ug/inverse-kinematics-for-robots-with-floating-base.html

I hope you find the above explanation and suggestions useful!

[Jonghyeok Kim]

Thank you for comments. It will be helpful in my situation, thanks.!

[Garmit Pant]

@Jonghyeok Kim Glad it helped!