diff --git a/src/examples/vx300.cpp b/src/examples/vx300.cpp new file mode 100644 index 00000000..0c158567 --- /dev/null +++ b/src/examples/vx300.cpp @@ -0,0 +1,39 @@ +#include <robot_dart/control/pd_control.hpp> +#include <robot_dart/robot_dart_simu.hpp> +#include <robot_dart/robots/vx300.hpp> + +#ifdef GRAPHIC +#include <robot_dart/gui/magnum/graphics.hpp> +#endif + +int main() +{ + auto robot = std::make_shared<robot_dart::robots::Vx300>(); + robot->set_actuator_types("servo"); + + Eigen::VectorXd ctrl = robot_dart::make_vector({0.0, 1.0, -1.5, 1.0, 0.5, 0.}); + + auto controller = std::make_shared<robot_dart::control::PDControl>(ctrl); + robot->add_controller(controller); + + robot_dart::RobotDARTSimu simu; + simu.set_collision_detector("fcl"); +#ifdef GRAPHIC + simu.set_graphics(std::make_shared<robot_dart::gui::magnum::Graphics>()); +#endif + simu.add_robot(robot); + simu.add_checkerboard_floor(); + + for (auto& n : robot->dof_names()) { + std::cout << n << std::endl; + } + + simu.run(2.5); + + ctrl << 0.0, -0.5, 0.5, -0.5, 0., 1.; + controller->set_parameters(ctrl); + controller->set_pd(20., 0.); + simu.run(2.5); + + return 0; +} diff --git a/src/python/robot.cpp b/src/python/robot.cpp index 33f1f65c..a25a6083 100644 --- a/src/python/robot.cpp +++ b/src/python/robot.cpp @@ -16,6 +16,7 @@ #include <robot_dart/robots/talos.hpp> #include <robot_dart/robots/tiago.hpp> #include <robot_dart/robots/ur3e.hpp> +#include <robot_dart/robots/vx300.hpp> #include <robot_dart/control/robot_control.hpp> @@ -598,6 +599,11 @@ namespace robot_dart { py::arg("frequency") = 1000, py::arg("urdf") = "ur3e/ur3e_with_schunk_hand.urdf", py::arg("packages") = std::vector<std::pair<std::string, std::string>>({{"ur3e_description", "ur3e/ur3e_description"}})); + + py::class_<Vx300, Robot, std::shared_ptr<Vx300>>(m, "Vx300") + .def(py::init<const std::string&, const std::vector<std::pair<std::string, std::string>>&>(), + py::arg("urdf") = "vx300/vx300.urdf", + py::arg("packages") = std::vector<std::pair<std::string, std::string>>({{"interbotix_xsarm_descriptions", "vx300"}})); } } // namespace python } // namespace robot_dart diff --git a/src/robot_dart/robots/vx300.hpp b/src/robot_dart/robots/vx300.hpp new file mode 100644 index 00000000..0b427de5 --- /dev/null +++ b/src/robot_dart/robots/vx300.hpp @@ -0,0 +1,19 @@ +#ifndef ROBOT_DART_ROBOTS_VX300_HPP +#define ROBOT_DART_ROBOTS_VX300_HPP + +#include "robot_dart/robot.hpp" + +namespace robot_dart { + namespace robots { + class Vx300 : public Robot { + public: + Vx300(const std::string& urdf = "vx300/vx300.urdf", const std::vector<std::pair<std::string, std::string>>& packages = {{"interbotix_xsarm_descriptions", "vx300"}}) : Robot(urdf, packages) + { + fix_to_world(); + set_position_enforced(true); + set_color_mode("aspect"); + } + }; + } // namespace robots +} // namespace robot_dart +#endif diff --git a/utheque/vx300/vx300.urdf b/utheque/vx300/vx300.urdf new file mode 100644 index 00000000..6d7325f7 --- /dev/null +++ b/utheque/vx300/vx300.urdf @@ -0,0 +1,322 @@ +<?xml version="1.0" ?> +<!-- =================================================================================== --> +<!-- | This document was autogenerated by xacro from urdf/vx300.urdf.xacro | --> +<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | --> +<!-- =================================================================================== --> +<robot name="vx300"> + <material name="interbotix_black"> + <texture filename="package://interbotix_xsarm_descriptions/interbotix_black.png"/> + </material> + <link name="vx300/base_link"> + <visual> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_1_base.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_1_base.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 1.5707963267948966" xyz="-0.0534774000 -0.0005625750 0.0205961000"/> + <mass value="0.969034"/> + <inertia ixx="0.0060240000" ixy="0.0000471300" ixz="0.0000038510" iyy="0.0017000000" iyz="-0.0000841500" izz="0.0071620000"/> + </inertial> + </link> + <joint name="waist" type="revolute"> + <axis xyz="0 0 1"/> + <limit effort="10" lower="-3.141582653589793" upper="3.141582653589793" velocity="3.141592653589793"/> + <origin rpy="0 0 0" xyz="0 0 0.079"/> + <parent link="vx300/base_link"/> + <child link="vx300/shoulder_link"/> + <dynamics friction="0.1"/> + </joint> + <link name="vx300/shoulder_link"> + <visual> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 -0.003"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_2_shoulder.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 -0.003"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_2_shoulder.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 1.5707963267948966" xyz="0.0002592330 -0.0000033552 0.0116129000"/> + <mass value="0.798614"/> + <inertia ixx="0.0009388000" ixy="-0.0000000010" ixz="-0.0000000191" iyy="0.0011380000" iyz="0.0000059568" izz="0.0012010000"/> + </inertial> + </link> + <joint name="shoulder" type="revolute"> + <axis xyz="0 1 0"/> + <limit effort="20" lower="-1.8500490071139892" upper="1.2566370614359172" velocity="3.141592653589793"/> + <origin rpy="0 0 0" xyz="0 0 0.04805"/> + <parent link="vx300/shoulder_link"/> + <child link="vx300/upper_arm_link"/> + <dynamics friction="0.1"/> + </joint> + <link name="vx300/upper_arm_link"> + <visual> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_3_upper_arm.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_3_upper_arm.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 1.5707963267948966" xyz="0.0206949000 0.0000000004 0.2264590000"/> + <mass value="0.792592"/> + <inertia ixx="0.0089250000" ixy="0.0000000000" ixz="0.0000000000" iyy="0.0089370000" iyz="0.0012010000" izz="0.0009357000"/> + </inertial> + </link> + <joint name="elbow" type="revolute"> + <axis xyz="0 1 0"/> + <limit effort="15" lower="-1.7627825445142729" upper="1.6057029118347832" velocity="3.141592653589793"/> + <origin rpy="0 0 0" xyz="0.05955 0 0.3"/> + <parent link="vx300/upper_arm_link"/> + <child link="vx300/forearm_link"/> + <dynamics friction="0.1"/> + </joint> + <link name="vx300/forearm_link"> + <visual> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_4_forearm.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_4_forearm.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 1.5707963267948966" xyz="0.1812650000 -0.0002065037 0.0000000000"/> + <mass value="0.544659"/> + <inertia ixx="0.0054470000" ixy="-0.0000119800" ixz="0.0000000000" iyy="0.0002267000" iyz="0.0000000000" izz="0.0055740000"/> + </inertial> + </link> + <joint name="wrist_angle" type="revolute"> + <axis xyz="0 1 0"/> + <limit effort="5" lower="-1.8675022996339325" upper="2.234021442552742" velocity="3.141592653589793"/> + <origin rpy="0 0 0" xyz="0.3 0 0"/> + <parent link="vx300/forearm_link"/> + <child link="vx300/wrist_link"/> + <dynamics friction="0.1"/> + </joint> + <link name="vx300/wrist_link"> + <visual> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_5_wrist.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="0 0 1.5707963267948966" xyz="0 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_5_wrist.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 1.5707963267948966" xyz="0.0467430000 -0.0000076652 0.0105650000"/> + <mass value="0.115395"/> + <inertia ixx="0.0000463100" ixy="0.0000000195" ixz="0.0000000023" iyy="0.0000451400" iyz="0.0000042002" izz="0.0000527000"/> + </inertial> + </link> + <joint name="wrist_rotate" type="revolute"> + <axis xyz="1 0 0"/> + <limit effort="1" lower="-3.141582653589793" upper="3.141582653589793" velocity="3.141592653589793"/> + <origin rpy="0 0 0" xyz="0.069744 0 0"/> + <parent link="vx300/wrist_link"/> + <child link="vx300/gripper_link"/> + <dynamics friction="0.1"/> + </joint> + <link name="vx300/gripper_link"> + <visual> + <origin rpy="0 0 1.5707963267948966" xyz="-0.02 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_6_gripper.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="0 0 1.5707963267948966" xyz="-0.02 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_6_gripper.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 1.5707963267948966" xyz="0.0230010000 0.0000000000 0.0115230000"/> + <mass value="0.097666"/> + <inertia ixx="0.0000326800" ixy="0.0000000000" ixz="0.0000000000" iyy="0.0000243600" iyz="0.0000002785" izz="0.0000211900"/> + </inertial> + </link> + <joint name="ee_arm" type="fixed"> + <axis xyz="1 0 0"/> + <origin rpy="0 0 0" xyz="0.042825 0 0"/> + <parent link="vx300/gripper_link"/> + <child link="vx300/ee_arm_link"/> + </joint> + <link name="vx300/ee_arm_link"> + <inertial> + <mass value="0.001"/> + <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/> + </inertial> + </link> + <!-- <joint name="gripper" type="continuous"> --> + <joint name="gripper" type="fixed"> + <axis xyz="1 0 0"/> + <!-- <limit effort="1" velocity="3.141592653589793"/> --> + <origin rpy="0 0 0" xyz="0.005675 0 0"/> + <parent link="vx300/ee_arm_link"/> + <child link="vx300/gripper_prop_link"/> + <!-- <dynamics friction="0.1"/> --> + </joint> + <link name="vx300/gripper_prop_link"> + <visual> + <origin rpy="0 0 1.5707963267948966" xyz="-0.0685 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_7_gripper_prop.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="0 0 1.5707963267948966" xyz="-0.0685 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_7_gripper_prop.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 1.5707963267948966" xyz="0.0023780000 0.0000000285 0.0000000000"/> + <mass value="0.008009"/> + <inertia ixx="0.0000020386" ixy="0.0000000000" ixz="0.0000006559" iyy="0.0000042979" iyz="0.0000000000" izz="0.0000023796"/> + </inertial> + </link> + <!-- If the AR tag is being used, then add the AR tag mount --> + <!-- If the gripper bar is being used, then also add the gripper bar --> + <joint name="gripper_bar" type="fixed"> + <axis xyz="1 0 0"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <parent link="vx300/ee_arm_link"/> + <child link="vx300/gripper_bar_link"/> + </joint> + <link name="vx300/gripper_bar_link"> + <visual> + <origin rpy="0 0 1.5707963267948966" xyz="-0.063 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_8_gripper_bar.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="0 0 1.5707963267948966" xyz="-0.063 0 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_8_gripper_bar.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 0 1.5707963267948966" xyz="0.0067940000 -0.0000004272 -0.0007760000"/> + <mass value="0.150986"/> + <inertia ixx="0.0000789500" ixy="-0.0000000012" ixz="0.0000001341" iyy="0.0003283000" iyz="0.0000017465" izz="0.0003095000"/> + </inertial> + </link> + <joint name="ee_bar" type="fixed"> + <axis xyz="1 0 0"/> + <origin rpy="0 0 0" xyz="0.025875 0 0"/> + <parent link="vx300/gripper_bar_link"/> + <child link="vx300/fingers_link"/> + </joint> + <link name="vx300/fingers_link"> + <inertial> + <mass value="0.001"/> + <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/> + </inertial> + </link> + <!-- If the gripper fingers are being used, add those as well --> + <joint name="left_finger" type="prismatic"> + <axis xyz="0 1 0"/> + <limit effort="5" lower="0.021" upper="0.057" velocity="1"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <parent link="vx300/fingers_link"/> + <child link="vx300/left_finger_link"/> + <dynamics friction="0.1"/> + </joint> + <link name="vx300/left_finger_link"> + <visual> + <origin rpy="1.5707963267948966 -3.141592653589793 1.5707963267948966" xyz="-0.0404 -0.0575 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_9_gripper_finger.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="1.5707963267948966 -3.141592653589793 1.5707963267948966" xyz="-0.0404 -0.0575 0"/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_9_gripper_finger.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="3.141592653589793 3.141592653589793 1.5707963267948966" xyz="0.0173440000 -0.0060692000 0.0000000000"/> + <mass value="0.034796"/> + <inertia ixx="0.0000243300" ixy="-0.0000024004" ixz="0.0000000000" iyy="0.0000125500" iyz="0.0000000000" izz="0.0000141700"/> + </inertial> + </link> + <joint name="right_finger" type="prismatic"> + <axis xyz="0 1 0"/> + <limit effort="5" lower="-0.057" upper="-0.021" velocity="1"/> + <origin rpy="0 0 0" xyz="0 0 0"/> + <parent link="vx300/fingers_link"/> + <child link="vx300/right_finger_link"/> + <dynamics friction="0.1"/> + <mimic joint="left_finger" multiplier="-1" offset="0"/> + </joint> + <link name="vx300/right_finger_link"> + <visual> + <origin rpy="-1.5707963267948966 3.141592653589793 -1.5707963267948966" xyz="-0.0404 0.0575 0 "/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_9_gripper_finger.stl" scale="0.001 0.001 0.001"/> + </geometry> + <material name="interbotix_black"/> + </visual> + <collision> + <origin rpy="-1.5707963267948966 3.141592653589793 -1.5707963267948966" xyz="-0.0404 0.0575 0 "/> + <geometry> + <mesh filename="package://interbotix_xsarm_descriptions/vx300_meshes/vx300_9_gripper_finger.stl" scale="0.001 0.001 0.001"/> + </geometry> + </collision> + <inertial> + <origin rpy="0 3.141592653589793 1.5707963267948966" xyz="0.0173440000 0.0060692000 0.0000000000"/> + <mass value="0.034796"/> + <inertia ixx="0.0000243300" ixy="0.0000024001" ixz="0.0000000000" iyy="0.0000125500" iyz="0.0000000000" izz="0.0000141700"/> + </inertial> + </link> + <joint name="ee_gripper" type="fixed"> + <axis xyz="1 0 0"/> + <origin rpy="0 0 0" xyz="0.0385 0 0"/> + <parent link="vx300/fingers_link"/> + <child link="vx300/ee_gripper_link"/> + </joint> + <link name="vx300/ee_gripper_link"> + <inertial> + <mass value="0.001"/> + <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/> + </inertial> + </link> +</robot> + diff --git a/utheque/vx300/vx300_meshes/vx300_10_ar_tag.stl b/utheque/vx300/vx300_meshes/vx300_10_ar_tag.stl new file mode 100644 index 00000000..193014b6 Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_10_ar_tag.stl differ diff --git a/utheque/vx300/vx300_meshes/vx300_1_base.stl b/utheque/vx300/vx300_meshes/vx300_1_base.stl new file mode 100644 index 00000000..5a7efda2 Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_1_base.stl differ diff --git a/utheque/vx300/vx300_meshes/vx300_2_shoulder.stl b/utheque/vx300/vx300_meshes/vx300_2_shoulder.stl new file mode 100644 index 00000000..dc22aa7e Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_2_shoulder.stl differ diff --git a/utheque/vx300/vx300_meshes/vx300_3_upper_arm.stl b/utheque/vx300/vx300_meshes/vx300_3_upper_arm.stl new file mode 100644 index 00000000..111c586e Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_3_upper_arm.stl differ diff --git a/utheque/vx300/vx300_meshes/vx300_4_forearm.stl b/utheque/vx300/vx300_meshes/vx300_4_forearm.stl new file mode 100644 index 00000000..cbdc35cf Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_4_forearm.stl differ diff --git a/utheque/vx300/vx300_meshes/vx300_5_wrist.stl b/utheque/vx300/vx300_meshes/vx300_5_wrist.stl new file mode 100644 index 00000000..ab8423e9 Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_5_wrist.stl differ diff --git a/utheque/vx300/vx300_meshes/vx300_6_gripper.stl b/utheque/vx300/vx300_meshes/vx300_6_gripper.stl new file mode 100644 index 00000000..043db9ca Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_6_gripper.stl differ diff --git a/utheque/vx300/vx300_meshes/vx300_7_gripper_prop.stl b/utheque/vx300/vx300_meshes/vx300_7_gripper_prop.stl new file mode 100644 index 00000000..36099b42 Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_7_gripper_prop.stl differ diff --git a/utheque/vx300/vx300_meshes/vx300_8_gripper_bar.stl b/utheque/vx300/vx300_meshes/vx300_8_gripper_bar.stl new file mode 100644 index 00000000..eba3caa2 Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_8_gripper_bar.stl differ diff --git a/utheque/vx300/vx300_meshes/vx300_9_gripper_finger.stl b/utheque/vx300/vx300_meshes/vx300_9_gripper_finger.stl new file mode 100644 index 00000000..d6df86be Binary files /dev/null and b/utheque/vx300/vx300_meshes/vx300_9_gripper_finger.stl differ