Parts of franka_panda not showing
matthiasobergasser opened this issue · 2 comments
matthiasobergasser commented
Hi
I tried to run the "tiny_urdf_parser_meshcat_example" with the franka_panda from https://github.com/bulletphysics/bullet3/tree/master/examples/pybullet/gym/pybullet_data/franka_panda
and "link5" , "link6" and "hand" are not shown (see picture)
tiny_urdf_parser_meshcat_example.cpp
// Copyright 2020 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <fstream>
#include <iostream>
#include <streambuf>
#include <string>
#include <thread>
#include "visualizer/meshcat/meshcat_urdf_visualizer.h"
#include "math/tiny/tiny_double_utils.h"
#include "utils/file_utils.hpp"
#include "urdf/urdf_parser.hpp"
#include "urdf/urdf_to_multi_body.hpp"
#include "dynamics/forward_dynamics.hpp"
#include "dynamics/integrator.hpp"
typedef double TinyDualScalar;
typedef double MyScalar;
typedef ::TINY::DoubleUtils MyTinyConstants;
#include "math/tiny/tiny_algebra.hpp"
typedef TinyAlgebra<double, MyTinyConstants> MyAlgebra;
using namespace tds;
using namespace TINY;
double knee_angle = 0.0;
double abduction_angle = 0.0;
int frameskip_gfx_sync = 16; // only sync every 10 frames (sim at 1000 Hz, gfx at ~60hz)
double initial_poses[] = {
abduction_angle,
0.,
knee_angle,
abduction_angle,
0.,
knee_angle,
abduction_angle,
0.,
knee_angle,
abduction_angle,
0.,
knee_angle,
};
int main(int argc, char *argv[])
{
World<MyAlgebra> world;
UrdfParser<MyAlgebra> parser;
// create graphics
MeshcatUrdfVisualizer<MyAlgebra> meshcat_viz;
meshcat_viz.delete_all();
std::string plane_file_name;
FileUtils::find_file("plane_implicit.urdf", plane_file_name);
char plane_search_path[TINY_MAX_EXE_PATH_LEN];
FileUtils::extract_path(plane_file_name.c_str(), plane_search_path, TINY_MAX_EXE_PATH_LEN);
MultiBody<MyAlgebra> &plane_mb = *world.create_multi_body();
plane_mb.set_floating_base(false);
{
UrdfStructures<MyAlgebra> plane_urdf_structures =
parser.load_urdf(plane_file_name);
UrdfToMultiBody<MyAlgebra>::convert_to_multi_body(
plane_urdf_structures, world, plane_mb);
std::string texture_path = "checker_purple.png";
meshcat_viz.m_path_prefix = plane_search_path;
meshcat_viz.convert_visuals(plane_urdf_structures, texture_path);
}
char search_path[TINY_MAX_EXE_PATH_LEN];
std::string file_name;
FileUtils::find_file("franka_panda/panda.urdf", file_name);
FileUtils::extract_path(file_name.c_str(), search_path, TINY_MAX_EXE_PATH_LEN);
std::ifstream ifs(file_name);
std::string urdf_string;
if (!ifs.is_open())
{
std::cout << "Error, cannot open file_name: " << file_name << std::endl;
exit(-1);
}
urdf_string = std::string((std::istreambuf_iterator<char>(ifs)), std::istreambuf_iterator<char>());
StdLogger logger;
UrdfStructures<MyAlgebra> urdf_structures;
int flags = 0;
parser.load_urdf_from_string(urdf_string, flags, logger, urdf_structures);
// create graphics structures
std::string texture_path = "./meshes/visual/colors.png";
meshcat_viz.m_path_prefix = search_path;
meshcat_viz.convert_visuals(urdf_structures, texture_path);
MultiBody<MyAlgebra> &mb = *world.create_multi_body();
UrdfToMultiBody<MyAlgebra>::convert_to_multi_body(urdf_structures, world, mb);
mb.initialize();
TinyVector3<double, DoubleUtils> grav (DoubleUtils::zero(), DoubleUtils::zero(), DoubleUtils::fraction(-981, 100));
double dt = 1. / 1000.;
int sync_counter = 0;
while (1)
{
std::this_thread::sleep_for(std::chrono::duration<double>(dt));
forward_kinematics(mb);
// pd control
if (1)
{
// use PD controller to compute tau
int qd_offset = mb.is_floating() ? 6 : 0;
int q_offset = mb.is_floating() ? 7 : 0;
int num_targets = mb.tau_.size() - qd_offset;
std::vector<double> q_targets;
q_targets.resize(mb.tau_.size());
double kp = 150;
double kd = 3;
double max_force = 550;
int param_index = 0;
for (int i = 0; i < mb.tau_.size(); i++)
{
mb.tau_[i] = 0;
}
int tau_index = 0;
int pose_index = 0;
for (int i = 0; i < mb.links_.size(); i++)
{
if (mb.links_[i].joint_type != JOINT_FIXED)
{
double q_desired = initial_poses[pose_index++];
double q_actual = mb.q_[q_offset];
double qd_actual = mb.qd_[qd_offset];
double position_error = (q_desired - q_actual);
double desired_velocity = 0;
double velocity_error = (desired_velocity - qd_actual);
double force = kp * position_error + kd * velocity_error;
if (force < -max_force)
force = -max_force;
if (force > max_force)
force = max_force;
mb.tau_[tau_index] = force;
q_offset++;
qd_offset++;
param_index++;
tau_index++;
}
}
}
forward_dynamics(mb, grav);
world.step(dt);
integrate_euler(mb, dt);
sync_counter++;
if (sync_counter > frameskip_gfx_sync)
{
sync_counter = 0;
meshcat_viz.sync_visual_transforms(&mb);
}
}
printf("finished\n");
return EXIT_SUCCESS;
}panda.urdf
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from panda_arm_hand.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="panda_link0">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<mass value="2.9"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/collision/link0.obj"/>
</geometry>
<material name="panda_white">
<color rgba="1. 1. 1. 1."/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/link0.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<link name="panda_link1">
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 -0.05"/>
<mass value="2.7"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/visual/link1.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/link1.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<joint name="panda_joint1" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="0 0 0" xyz="0 0 0.333"/>
<parent link="panda_link0"/>
<child link="panda_link1"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
</joint>
<link name="panda_link2">
<inertial>
<origin rpy="0 0 0" xyz="0 -0.04 0.06"/>
<mass value="2.73"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/visual/link2.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/link2.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<joint name="panda_joint2" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
<parent link="panda_link1"/>
<child link="panda_link2"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-1.8326" upper="1.8326" velocity="2.1750"/>
</joint>
<link name="panda_link3">
<inertial>
<origin rpy="0 0 0" xyz="0.01 0.01 -0.05"/>
<mass value="2.04"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/visual/link3.obj"/>
</geometry>
<material name="panda_red">
<color rgba="1. 1. 1. 1."/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/link3.obj"/>
</geometry>
</collision>
</link>
<joint name="panda_joint3" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
<parent link="panda_link2"/>
<child link="panda_link3"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
</joint>
<link name="panda_link4">
<inertial>
<origin rpy="0 0 0" xyz="-0.03 0.03 0.02"/>
<mass value="2.08"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/visual/link4.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/link4.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<joint name="panda_joint4" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
<parent link="panda_link3"/>
<child link="panda_link4"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-3.1416" upper="0.0" velocity="2.1750"/>
</joint>
<link name="panda_link5">
<inertial>
<origin rpy="0 0 0" xyz="0 0.04 -0.12"/>
<mass value="3"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/visual/link5.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/link5.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<joint name="panda_joint5" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
<parent link="panda_link4"/>
<child link="panda_link5"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
</joint>
<link name="panda_link6">
<inertial>
<origin rpy="0 0 0" xyz="0.04 0 0"/>
<mass value="1.3"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/visual/link6.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/link6.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<joint name="panda_joint6" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<parent link="panda_link5"/>
<child link="panda_link6"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-0.0873" upper="3.8223" velocity="2.6100"/>
</joint>
<link name="panda_link7">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.08"/>
<mass value=".2"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/collision/link7.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/link7.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<joint name="panda_joint7" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
<parent link="panda_link6"/>
<child link="panda_link7"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
</joint>
<link name="panda_link8">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
</link>
<joint name="panda_joint8" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.107"/>
<parent link="panda_link7"/>
<child link="panda_link8"/>
<axis xyz="0 0 0"/>
</joint>
<joint name="panda_hand_joint" type="fixed">
<parent link="panda_link8"/>
<child link="panda_hand"/>
<origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
</joint>
<link name="panda_hand">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.04"/>
<mass value=".81"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/visual/hand.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/hand.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<link name="panda_leftfinger">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0.01 0.02"/>
<mass value="0.1"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<geometry>
<mesh filename="./meshes/visual/finger.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<geometry>
<mesh filename="./meshes/collision/finger.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<link name="panda_rightfinger">
<contact>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.01 0.02"/>
<mass value="0.1"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="./meshes/visual/finger.obj"/>
</geometry>
<material name="panda_white"/>
</visual>
<collision>
<origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
<geometry>
<mesh filename="./meshes/collision/finger.obj"/>
</geometry>
<material name="panda_white"/>
</collision>
</link>
<joint name="panda_finger_joint1" type="prismatic">
<parent link="panda_hand"/>
<child link="panda_leftfinger"/>
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
<axis xyz="0 1 0"/>
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
</joint>
<joint name="panda_finger_joint2" type="prismatic">
<parent link="panda_hand"/>
<child link="panda_rightfinger"/>
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
<axis xyz="0 -1 0"/>
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
<mimic joint="panda_finger_joint1"/>
</joint>
<link name="panda_grasptarget">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
</link>
<joint name="panda_grasptarget_hand" type="fixed">
<parent link="panda_hand"/>
<child link="panda_grasptarget"/>
<origin rpy="0 0 0" xyz="0 0 0.105"/>
</joint>
</robot>
matthiasobergasser commented
found another version of the meshes and it works now
matthiasobergasser commented
Quick Fix: If you open the files in MeshLab, export the failing visual meshes again to .obj and they should appear
(don't know what exactly happens but it works)