function robot = exampleHelperWGPR2Kinect()
%exampleHelperWGPR2Kinect create a PR2 robot model

% Copyright 2016-2019 The MathWorks, Inc.

robot = rigidBodyTree();
% Add body, 'base_footprint', and joint, 'base_footprint_jnt'
robot.BaseName = 'base_footprint';


% Add body, 'base_link', and joint, 'base_footprint_joint'
bodyName = 'base_link';
parentName = 'base_footprint';
jointName = 'base_footprint_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                  0.051; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'base_bellow_link', and joint, 'base_bellow_joint'
bodyName = 'base_bellow_link';
parentName = 'base_link';
jointName = 'base_bellow_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                  -0.29; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                    0.8; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'base_laser_link', and joint, 'base_laser_joint'
bodyName = 'base_laser_link';
parentName = 'base_link';
jointName = 'base_laser_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                  0.275; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                  0.252; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'fl_caster_rotation_link', and joint, 'fl_caster_rotation_joint'
bodyName = 'fl_caster_rotation_link';
parentName = 'base_link';
jointName = 'fl_caster_rotation_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                 0.2246; ...
                                          0,                      1,                      0,                 0.2246; ...
                                         -0,                      0,                      1,                 0.0282; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'fl_caster_l_wheel_link', and joint, 'fl_caster_l_wheel_joint'
bodyName = 'fl_caster_l_wheel_link';
parentName = 'fl_caster_rotation_link';
jointName = 'fl_caster_l_wheel_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                  0.049; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'fl_caster_r_wheel_link', and joint, 'fl_caster_r_wheel_joint'
bodyName = 'fl_caster_r_wheel_link';
parentName = 'fl_caster_rotation_link';
jointName = 'fl_caster_r_wheel_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                 -0.049; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'fr_caster_rotation_link', and joint, 'fr_caster_rotation_joint'
bodyName = 'fr_caster_rotation_link';
parentName = 'base_link';
jointName = 'fr_caster_rotation_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                 0.2246; ...
                                          0,                      1,                      0,                -0.2246; ...
                                         -0,                      0,                      1,                 0.0282; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'fr_caster_l_wheel_link', and joint, 'fr_caster_l_wheel_joint'
bodyName = 'fr_caster_l_wheel_link';
parentName = 'fr_caster_rotation_link';
jointName = 'fr_caster_l_wheel_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                  0.049; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'fr_caster_r_wheel_link', and joint, 'fr_caster_r_wheel_joint'
bodyName = 'fr_caster_r_wheel_link';
parentName = 'fr_caster_rotation_link';
jointName = 'fr_caster_r_wheel_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                 -0.049; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'bl_caster_rotation_link', and joint, 'bl_caster_rotation_joint'
bodyName = 'bl_caster_rotation_link';
parentName = 'base_link';
jointName = 'bl_caster_rotation_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                -0.2246; ...
                                          0,                      1,                      0,                 0.2246; ...
                                         -0,                      0,                      1,                 0.0282; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'bl_caster_l_wheel_link', and joint, 'bl_caster_l_wheel_joint'
bodyName = 'bl_caster_l_wheel_link';
parentName = 'bl_caster_rotation_link';
jointName = 'bl_caster_l_wheel_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                  0.049; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'bl_caster_r_wheel_link', and joint, 'bl_caster_r_wheel_joint'
bodyName = 'bl_caster_r_wheel_link';
parentName = 'bl_caster_rotation_link';
jointName = 'bl_caster_r_wheel_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                 -0.049; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'br_caster_rotation_link', and joint, 'br_caster_rotation_joint'
bodyName = 'br_caster_rotation_link';
parentName = 'base_link';
jointName = 'br_caster_rotation_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                -0.2246; ...
                                          0,                      1,                      0,                -0.2246; ...
                                         -0,                      0,                      1,                 0.0282; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'br_caster_l_wheel_link', and joint, 'br_caster_l_wheel_joint'
bodyName = 'br_caster_l_wheel_link';
parentName = 'br_caster_rotation_link';
jointName = 'br_caster_l_wheel_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                  0.049; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'br_caster_r_wheel_link', and joint, 'br_caster_r_wheel_joint'
bodyName = 'br_caster_r_wheel_link';
parentName = 'br_caster_rotation_link';
jointName = 'br_caster_r_wheel_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                 -0.049; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'torso_lift_link', and joint, 'torso_lift_joint'
bodyName = 'torso_lift_link';
parentName = 'base_link';
jointName = 'torso_lift_joint';
jointType = 'prismatic';
T_Joint_to_Parent = [                     1,                      0,                      0,                  -0.05; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,               0.739675; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [                     0,                   0.33];
jointHomePosition =                  0.165;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_torso_lift_side_plate_link', and joint, 'l_torso_lift_side_plate_joint'
bodyName = 'l_torso_lift_side_plate_link';
parentName = 'torso_lift_link';
jointName = 'l_torso_lift_side_plate_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                 0.0535; ...
                                          0,                      1,                      0,               0.209285; ...
                                         -0,                      0,                      1,               0.176625; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_torso_lift_side_plate_link', and joint, 'r_torso_lift_side_plate_joint'
bodyName = 'r_torso_lift_side_plate_link';
parentName = 'torso_lift_link';
jointName = 'r_torso_lift_side_plate_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                 0.0535; ...
                                          0,                      1,                      0,              -0.209285; ...
                                         -0,                      0,                      1,               0.176625; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'imu_link', and joint, 'imu_joint'
bodyName = 'imu_link';
parentName = 'torso_lift_link';
jointName = 'imu_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                    -1,                     -0, -2.068231071102144e-13,               -0.02977; ...
                                         -0,                      1,                     -0,                -0.1497; ...
                      2.068231071102144e-13,                     -0,                     -1,                  0.164; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_pan_link', and joint, 'head_pan_joint'
bodyName = 'head_pan_link';
parentName = 'torso_lift_link';
jointName = 'head_pan_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,               -0.01707; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                0.38145; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [                -3.007,                  3.007];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_tilt_link', and joint, 'head_tilt_joint'
bodyName = 'head_tilt_link';
parentName = 'head_pan_link';
jointName = 'head_tilt_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,    0.06800000000000001; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [             -0.471238,                1.39626];
jointHomePosition =               0.462511;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_plate_frame', and joint, 'head_plate_frame_joint'
bodyName = 'head_plate_frame';
parentName = 'head_tilt_link';
jointName = 'head_plate_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                 0.0232; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                 0.0645; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'sensor_mount_link', and joint, 'sensor_mount_frame_joint'
bodyName = 'sensor_mount_link';
parentName = 'head_plate_frame';
jointName = 'sensor_mount_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'high_def_frame', and joint, 'high_def_frame_joint'
bodyName = 'high_def_frame';
parentName = 'sensor_mount_link';
jointName = 'high_def_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,               0.046457; ...
                                          0,                      1,                      0,                  -0.11; ...
                                         -0,                      0,                      1,                 0.0546; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'high_def_optical_frame', and joint, 'high_def_optical_frame_joint'
bodyName = 'high_def_optical_frame';
parentName = 'high_def_frame';
jointName = 'high_def_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'double_stereo_link', and joint, 'double_stereo_frame_joint'
bodyName = 'double_stereo_link';
parentName = 'sensor_mount_link';
jointName = 'double_stereo_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'wide_stereo_link', and joint, 'wide_stereo_frame_joint'
bodyName = 'wide_stereo_link';
parentName = 'double_stereo_link';
jointName = 'wide_stereo_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                  0.045; ...
                                          0,                      1,                      0,                   0.03; ...
                                         -0,                      0,                      1,                 0.0501; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'wide_stereo_optical_frame', and joint, 'wide_stereo_optical_frame_joint'
bodyName = 'wide_stereo_optical_frame';
parentName = 'wide_stereo_link';
jointName = 'wide_stereo_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'wide_stereo_l_stereo_camera_frame', and joint, 'wide_stereo_l_stereo_camera_frame_joint'
bodyName = 'wide_stereo_l_stereo_camera_frame';
parentName = 'wide_stereo_link';
jointName = 'wide_stereo_l_stereo_camera_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'wide_stereo_l_stereo_camera_optical_frame', and joint, 'wide_stereo_l_stereo_camera_optical_frame_joint'
bodyName = 'wide_stereo_l_stereo_camera_optical_frame';
parentName = 'wide_stereo_l_stereo_camera_frame';
jointName = 'wide_stereo_l_stereo_camera_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'wide_stereo_r_stereo_camera_frame', and joint, 'wide_stereo_r_stereo_camera_frame_joint'
bodyName = 'wide_stereo_r_stereo_camera_frame';
parentName = 'wide_stereo_l_stereo_camera_frame';
jointName = 'wide_stereo_r_stereo_camera_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                  -0.09; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'wide_stereo_r_stereo_camera_optical_frame', and joint, 'wide_stereo_r_stereo_camera_optical_frame_joint'
bodyName = 'wide_stereo_r_stereo_camera_optical_frame';
parentName = 'wide_stereo_r_stereo_camera_frame';
jointName = 'wide_stereo_r_stereo_camera_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'narrow_stereo_link', and joint, 'narrow_stereo_frame_joint'
bodyName = 'narrow_stereo_link';
parentName = 'double_stereo_link';
jointName = 'narrow_stereo_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                  0.045; ...
                                          0,                      1,                      0,                   0.06; ...
                                         -0,                      0,                      1,                 0.0501; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'narrow_stereo_optical_frame', and joint, 'narrow_stereo_optical_frame_joint'
bodyName = 'narrow_stereo_optical_frame';
parentName = 'narrow_stereo_link';
jointName = 'narrow_stereo_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'narrow_stereo_l_stereo_camera_frame', and joint, 'narrow_stereo_l_stereo_camera_frame_joint'
bodyName = 'narrow_stereo_l_stereo_camera_frame';
parentName = 'narrow_stereo_link';
jointName = 'narrow_stereo_l_stereo_camera_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'narrow_stereo_l_stereo_camera_optical_frame', and joint, 'narrow_stereo_l_stereo_camera_optical_frame_joint'
bodyName = 'narrow_stereo_l_stereo_camera_optical_frame';
parentName = 'narrow_stereo_l_stereo_camera_frame';
jointName = 'narrow_stereo_l_stereo_camera_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'narrow_stereo_r_stereo_camera_frame', and joint, 'narrow_stereo_r_stereo_camera_frame_joint'
bodyName = 'narrow_stereo_r_stereo_camera_frame';
parentName = 'narrow_stereo_l_stereo_camera_frame';
jointName = 'narrow_stereo_r_stereo_camera_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                  -0.09; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'narrow_stereo_r_stereo_camera_optical_frame', and joint, 'narrow_stereo_r_stereo_camera_optical_frame_joint'
bodyName = 'narrow_stereo_r_stereo_camera_optical_frame';
parentName = 'narrow_stereo_r_stereo_camera_frame';
jointName = 'narrow_stereo_r_stereo_camera_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'projector_wg6802418_frame', and joint, 'projector_wg6802418_frame_joint'
bodyName = 'projector_wg6802418_frame';
parentName = 'head_plate_frame';
jointName = 'projector_wg6802418_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                   0.11; ...
                                         -0,                      0,                      1,                 0.0546; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'projector_wg6802418_child_frame', and joint, 'projector_wg6802418_child_frame_joint'
bodyName = 'projector_wg6802418_child_frame';
parentName = 'projector_wg6802418_frame';
jointName = 'projector_wg6802418_child_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,                     -0,                     -1,                      0; ...
                                          0,                      1,                     -0,                      0; ...
                                          1,                      0,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_mount_link', and joint, 'head_mount_joint'
bodyName = 'head_mount_link';
parentName = 'head_plate_frame';
jointName = 'head_mount_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                 -0.138; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                  0.091; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_mount_kinect_ir_link', and joint, 'head_mount_kinect_ir_joint'
bodyName = 'head_mount_kinect_ir_link';
parentName = 'head_mount_link';
jointName = 'head_mount_kinect_ir_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,              -0.032267; ...
                                          0,                      1,                      0,                 0.0125; ...
                                         -0,                      0,                      1,               0.136453; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_mount_kinect_ir_optical_frame', and joint, 'head_mount_kinect_ir_optical_frame_joint'
bodyName = 'head_mount_kinect_ir_optical_frame';
parentName = 'head_mount_kinect_ir_link';
jointName = 'head_mount_kinect_ir_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                   0.01; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_mount_kinect_rgb_link', and joint, 'head_mount_kinect_rgb_joint'
bodyName = 'head_mount_kinect_rgb_link';
parentName = 'head_mount_kinect_ir_link';
jointName = 'head_mount_kinect_rgb_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                  -0.03; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_mount_kinect_rgb_optical_frame', and joint, 'head_mount_kinect_rgb_optical_frame_joint'
bodyName = 'head_mount_kinect_rgb_optical_frame';
parentName = 'head_mount_kinect_rgb_link';
jointName = 'head_mount_kinect_rgb_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_mount_prosilica_link', and joint, 'head_mount_prosilica_joint'
bodyName = 'head_mount_prosilica_link';
parentName = 'head_mount_link';
jointName = 'head_mount_prosilica_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,              -0.046457; ...
                                          0,                      1,                      0,                 0.0125; ...
                                         -0,                      0,                      1,               0.088921; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'head_mount_prosilica_optical_frame', and joint, 'head_mount_prosilica_optical_frame_joint'
bodyName = 'head_mount_prosilica_optical_frame';
parentName = 'head_mount_prosilica_link';
jointName = 'head_mount_prosilica_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'laser_tilt_mount_link', and joint, 'laser_tilt_mount_joint'
bodyName = 'laser_tilt_mount_link';
parentName = 'torso_lift_link';
jointName = 'laser_tilt_mount_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                0.09893; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                  0.227; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [               -0.7854,                1.48353];
jointHomePosition =               0.349065;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'laser_tilt_link', and joint, 'laser_tilt_joint'
bodyName = 'laser_tilt_link';
parentName = 'laser_tilt_mount_link';
jointName = 'laser_tilt_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                   0.03; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_shoulder_pan_link', and joint, 'r_shoulder_pan_joint'
bodyName = 'r_shoulder_pan_link';
parentName = 'torso_lift_link';
jointName = 'r_shoulder_pan_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                 -0.188; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [         -2.2853981634,         0.714601836603];
jointHomePosition =       -0.7853981633985;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_shoulder_lift_link', and joint, 'r_shoulder_lift_joint'
bodyName = 'r_shoulder_lift_link';
parentName = 'r_shoulder_pan_link';
jointName = 'r_shoulder_lift_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                    0.1; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [               -0.5236,                 1.3963];
jointHomePosition =     0.4363500000000001;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_upper_arm_roll_link', and joint, 'r_upper_arm_roll_joint'
bodyName = 'r_upper_arm_roll_link';
parentName = 'r_shoulder_lift_link';
jointName = 'r_upper_arm_roll_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     1,                      0,                      0];
jointPositionLimits = [                  -3.9,                    0.8];
jointHomePosition =                  -1.55;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_upper_arm_link', and joint, 'r_upper_arm_joint'
bodyName = 'r_upper_arm_link';
parentName = 'r_upper_arm_roll_link';
jointName = 'r_upper_arm_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_elbow_flex_link', and joint, 'r_elbow_flex_joint'
bodyName = 'r_elbow_flex_link';
parentName = 'r_upper_arm_link';
jointName = 'r_elbow_flex_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                    0.4; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [               -2.3213,                      0];
jointHomePosition =               -1.16065;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_forearm_roll_link', and joint, 'r_forearm_roll_joint'
bodyName = 'r_forearm_roll_link';
parentName = 'r_elbow_flex_link';
jointName = 'r_forearm_roll_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     1,                      0,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_forearm_link', and joint, 'r_forearm_joint'
bodyName = 'r_forearm_link';
parentName = 'r_forearm_roll_link';
jointName = 'r_forearm_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_wrist_flex_link', and joint, 'r_wrist_flex_joint'
bodyName = 'r_wrist_flex_link';
parentName = 'r_forearm_link';
jointName = 'r_wrist_flex_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                  0.321; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [                 -2.18,                      0];
jointHomePosition =                  -1.09;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_wrist_roll_link', and joint, 'r_wrist_roll_joint'
bodyName = 'r_wrist_roll_link';
parentName = 'r_wrist_flex_link';
jointName = 'r_wrist_roll_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     1,                      0,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_palm_link', and joint, 'r_gripper_palm_joint'
bodyName = 'r_gripper_palm_link';
parentName = 'r_wrist_roll_link';
jointName = 'r_gripper_palm_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_led_frame', and joint, 'r_gripper_led_joint'
bodyName = 'r_gripper_led_frame';
parentName = 'r_gripper_palm_link';
jointName = 'r_gripper_led_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                 0.0513; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                 0.0244; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_motor_accelerometer_link', and joint, 'r_gripper_motor_accelerometer_joint'
bodyName = 'r_gripper_motor_accelerometer_link';
parentName = 'r_gripper_palm_link';
jointName = 'r_gripper_motor_accelerometer_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_tool_frame', and joint, 'r_gripper_tool_joint'
bodyName = 'r_gripper_tool_frame';
parentName = 'r_gripper_palm_link';
jointName = 'r_gripper_tool_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                   0.18; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_motor_slider_link', and joint, 'r_gripper_motor_slider_joint'
bodyName = 'r_gripper_motor_slider_link';
parentName = 'r_gripper_palm_link';
jointName = 'r_gripper_motor_slider_joint';
jointType = 'prismatic';
T_Joint_to_Parent = [                     1,                      0,                      0,                0.16828; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     1,                      0,                      0];
jointPositionLimits = [                  -0.1,                    0.1];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_motor_screw_link', and joint, 'r_gripper_motor_screw_joint'
bodyName = 'r_gripper_motor_screw_link';
parentName = 'r_gripper_motor_slider_link';
jointName = 'r_gripper_motor_screw_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_l_finger_link', and joint, 'r_gripper_l_finger_joint'
bodyName = 'r_gripper_l_finger_link';
parentName = 'r_gripper_palm_link';
jointName = 'r_gripper_l_finger_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,    0.07691000000000001; ...
                                          0,                      1,                      0,                   0.01; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [                     0,                  0.548];
jointHomePosition =                  0.274;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_l_finger_tip_link', and joint, 'r_gripper_l_finger_tip_joint'
bodyName = 'r_gripper_l_finger_tip_link';
parentName = 'r_gripper_l_finger_link';
jointName = 'r_gripper_l_finger_tip_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,    0.09137000000000001; ...
                                          0,                      1,                      0,                0.00495; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                     -1];
jointPositionLimits = [                     0,                  0.548];
jointHomePosition =                  0.274;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_r_finger_link', and joint, 'r_gripper_r_finger_joint'
bodyName = 'r_gripper_r_finger_link';
parentName = 'r_gripper_palm_link';
jointName = 'r_gripper_r_finger_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,    0.07691000000000001; ...
                                          0,                      1,                      0,                  -0.01; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                     -1];
jointPositionLimits = [                     0,                  0.548];
jointHomePosition =                  0.274;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_r_finger_tip_link', and joint, 'r_gripper_r_finger_tip_joint'
bodyName = 'r_gripper_r_finger_tip_link';
parentName = 'r_gripper_r_finger_link';
jointName = 'r_gripper_r_finger_tip_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,    0.09137000000000001; ...
                                          0,                      1,                      0,               -0.00495; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [                     0,                  0.548];
jointHomePosition =                  0.274;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_gripper_l_finger_tip_frame', and joint, 'r_gripper_joint'
bodyName = 'r_gripper_l_finger_tip_frame';
parentName = 'r_gripper_r_finger_tip_link';
jointName = 'r_gripper_joint';
jointType = 'prismatic';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [                     0,                   0.09];
jointHomePosition =                  0.045;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_forearm_cam_frame', and joint, 'r_forearm_cam_frame_joint'
bodyName = 'r_forearm_cam_frame';
parentName = 'r_forearm_roll_link';
jointName = 'r_forearm_cam_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [    0.8457278217040647,    -0.5336145159154667, -2.612890894244274e-12,                  0.135; ...
                                          0,  4.896588860146748e-12,                     -1,                      0; ...
                         0.5336145159154667,     0.8457278217040647,  4.141181430472298e-12,                  0.044; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'r_forearm_cam_optical_frame', and joint, 'r_forearm_cam_optical_frame_joint'
bodyName = 'r_forearm_cam_optical_frame';
parentName = 'r_forearm_cam_frame';
jointName = 'r_forearm_cam_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_shoulder_pan_link', and joint, 'l_shoulder_pan_joint'
bodyName = 'l_shoulder_pan_link';
parentName = 'torso_lift_link';
jointName = 'l_shoulder_pan_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                  0.188; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [       -0.714601836603,           2.2853981634];
jointHomePosition =        0.7853981633985;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_shoulder_lift_link', and joint, 'l_shoulder_lift_joint'
bodyName = 'l_shoulder_lift_link';
parentName = 'l_shoulder_pan_link';
jointName = 'l_shoulder_lift_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                    0.1; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [               -0.5236,                 1.3963];
jointHomePosition =     0.4363500000000001;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_upper_arm_roll_link', and joint, 'l_upper_arm_roll_joint'
bodyName = 'l_upper_arm_roll_link';
parentName = 'l_shoulder_lift_link';
jointName = 'l_upper_arm_roll_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     1,                      0,                      0];
jointPositionLimits = [                  -0.8,                    3.9];
jointHomePosition =                   1.55;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_upper_arm_link', and joint, 'l_upper_arm_joint'
bodyName = 'l_upper_arm_link';
parentName = 'l_upper_arm_roll_link';
jointName = 'l_upper_arm_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_elbow_flex_link', and joint, 'l_elbow_flex_joint'
bodyName = 'l_elbow_flex_link';
parentName = 'l_upper_arm_link';
jointName = 'l_elbow_flex_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                    0.4; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [               -2.3213,                      0];
jointHomePosition =               -1.16065;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_forearm_roll_link', and joint, 'l_forearm_roll_joint'
bodyName = 'l_forearm_roll_link';
parentName = 'l_elbow_flex_link';
jointName = 'l_forearm_roll_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     1,                      0,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_forearm_link', and joint, 'l_forearm_joint'
bodyName = 'l_forearm_link';
parentName = 'l_forearm_roll_link';
jointName = 'l_forearm_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_wrist_flex_link', and joint, 'l_wrist_flex_joint'
bodyName = 'l_wrist_flex_link';
parentName = 'l_forearm_link';
jointName = 'l_wrist_flex_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                  0.321; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [                 -2.18,                      0];
jointHomePosition =                  -1.09;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_wrist_roll_link', and joint, 'l_wrist_roll_joint'
bodyName = 'l_wrist_roll_link';
parentName = 'l_wrist_flex_link';
jointName = 'l_wrist_roll_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     1,                      0,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_palm_link', and joint, 'l_gripper_palm_joint'
bodyName = 'l_gripper_palm_link';
parentName = 'l_wrist_roll_link';
jointName = 'l_gripper_palm_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_led_frame', and joint, 'l_gripper_led_joint'
bodyName = 'l_gripper_led_frame';
parentName = 'l_gripper_palm_link';
jointName = 'l_gripper_led_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                 0.0513; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                 0.0244; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_motor_accelerometer_link', and joint, 'l_gripper_motor_accelerometer_joint'
bodyName = 'l_gripper_motor_accelerometer_link';
parentName = 'l_gripper_palm_link';
jointName = 'l_gripper_motor_accelerometer_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_tool_frame', and joint, 'l_gripper_tool_joint'
bodyName = 'l_gripper_tool_frame';
parentName = 'l_gripper_palm_link';
jointName = 'l_gripper_tool_joint';
jointType = 'fixed';
T_Joint_to_Parent = [                     1,                      0,                      0,                   0.18; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_motor_slider_link', and joint, 'l_gripper_motor_slider_joint'
bodyName = 'l_gripper_motor_slider_link';
parentName = 'l_gripper_palm_link';
jointName = 'l_gripper_motor_slider_joint';
jointType = 'prismatic';
T_Joint_to_Parent = [                     1,                      0,                      0,                0.16828; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     1,                      0,                      0];
jointPositionLimits = [                  -0.1,                    0.1];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_motor_screw_link', and joint, 'l_gripper_motor_screw_joint'
bodyName = 'l_gripper_motor_screw_link';
parentName = 'l_gripper_motor_slider_link';
jointName = 'l_gripper_motor_screw_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_l_finger_link', and joint, 'l_gripper_l_finger_joint'
bodyName = 'l_gripper_l_finger_link';
parentName = 'l_gripper_palm_link';
jointName = 'l_gripper_l_finger_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,    0.07691000000000001; ...
                                          0,                      1,                      0,                   0.01; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [                     0,                  0.548];
jointHomePosition =                  0.274;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_l_finger_tip_link', and joint, 'l_gripper_l_finger_tip_joint'
bodyName = 'l_gripper_l_finger_tip_link';
parentName = 'l_gripper_l_finger_link';
jointName = 'l_gripper_l_finger_tip_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,    0.09137000000000001; ...
                                          0,                      1,                      0,                0.00495; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                     -1];
jointPositionLimits = [                     0,                  0.548];
jointHomePosition =                  0.274;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_r_finger_link', and joint, 'l_gripper_r_finger_joint'
bodyName = 'l_gripper_r_finger_link';
parentName = 'l_gripper_palm_link';
jointName = 'l_gripper_r_finger_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,    0.07691000000000001; ...
                                          0,                      1,                      0,                  -0.01; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                     -1];
jointPositionLimits = [                     0,                  0.548];
jointHomePosition =                  0.274;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_r_finger_tip_link', and joint, 'l_gripper_r_finger_tip_joint'
bodyName = 'l_gripper_r_finger_tip_link';
parentName = 'l_gripper_r_finger_link';
jointName = 'l_gripper_r_finger_tip_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,    0.09137000000000001; ...
                                          0,                      1,                      0,               -0.00495; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [                     0,                  0.548];
jointHomePosition =                  0.274;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_gripper_l_finger_tip_frame', and joint, 'l_gripper_joint'
bodyName = 'l_gripper_l_finger_tip_frame';
parentName = 'l_gripper_r_finger_tip_link';
jointName = 'l_gripper_joint';
jointType = 'prismatic';
T_Joint_to_Parent = [                     1,                      0,                      0,                      0; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      1,                      0];
jointPositionLimits = [                     0,                   0.09];
jointHomePosition =                  0.045;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_forearm_cam_frame', and joint, 'l_forearm_cam_frame_joint'
bodyName = 'l_forearm_cam_frame';
parentName = 'l_forearm_roll_link';
jointName = 'l_forearm_cam_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [    0.8457278217040647,     0.5336145159154667, -2.612890894244274e-12,                  0.135; ...
                                          0,  4.896588860146748e-12,                      1,                      0; ...
                         0.5336145159154667,    -0.8457278217040647,  4.141181430472298e-12,                  0.044; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'l_forearm_cam_optical_frame', and joint, 'l_forearm_cam_optical_frame_joint'
bodyName = 'l_forearm_cam_optical_frame';
parentName = 'l_forearm_cam_frame';
jointName = 'l_forearm_cam_optical_frame_joint';
jointType = 'fixed';
T_Joint_to_Parent = [ 4.896588860146748e-12,  4.896588860146748e-12,                      1,                      0; ...
                                         -1,  2.397658246531323e-23,  4.896588860146748e-12,                      0; ...
                                         -0,                     -1,  4.896588860146748e-12,                      0; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


% Add body, 'torso_lift_motor_screw_link', and joint, 'torso_lift_motor_screw_joint'
bodyName = 'torso_lift_motor_screw_link';
parentName = 'base_link';
jointName = 'torso_lift_motor_screw_joint';
jointType = 'revolute';
T_Joint_to_Parent = [                     1,                      0,                      0,                  -0.15; ...
                                          0,                      1,                      0,                      0; ...
                                         -0,                      0,                      1,                    0.7; ...
                                          0,                      0,                      0,                      1];

joint = rigidBodyJoint(jointName, jointType);
joint.setFixedTransform(T_Joint_to_Parent);
jointAxis = [                     0,                      0,                      1];
jointPositionLimits = [    -3.141592653589793,      3.141592653589793];
jointHomePosition =                      0;
joint.PositionLimits = jointPositionLimits;
joint.JointAxis = jointAxis;
joint.HomePosition = jointHomePosition;

body = rigidBody(bodyName);
body.Joint = joint;
robot.addBody(body, parentName);


