mirror of
https://github.com/wassname/kair_algorithms_draft.git
synced 2026-09-09 11:25:10 +08:00
Replace the fk solver with kdl kinematics (#53)
* Replace the fk solver with kdl kinematics * Resolve the wrong kdl kinematics due to ill robot tree structure * Apply isort and black and apply some naming changes * Remove white spaces, blanks * Change file permission 755 to 644 * Change file permission 755 to 644 * Change file permission 755 to 644 * Change file permission 755 to 644 * Change file permission 644 to 755
This commit is contained in:
Cheolhui
@@ -7,10 +7,11 @@
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
|
||||
<!-- rviz & tf related args -->
|
||||
<arg name="robot_name" default="open_manipulator"/>
|
||||
<arg name="open_rviz" default="false" />
|
||||
<arg name="use_gui" default="false" />
|
||||
<!-- robot URDF parse args -->
|
||||
<arg name="om_urdf" value="robot_description"/>
|
||||
<arg name="urdf_param" default="/robot_description"/>
|
||||
<param name="$(arg om_urdf)" textfile="$(find kair_algorithms)/urdf/open_manipulator.urdf"/>
|
||||
<arg name="load_robot_description" default="false"/>
|
||||
|
||||
<!-- gazebo related -->
|
||||
<rosparam file="$(find open_manipulator_gazebo)/config/gazebo_controller.yaml" command="load" />
|
||||
@@ -23,28 +24,17 @@
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<!-- rviz related -->
|
||||
<!-- Send joint values -->
|
||||
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
|
||||
<param name="/use_gui" value="$(arg use_gui)"/>
|
||||
<rosparam param="source_list" subst_value="true">["$(arg robot_name)/joint_states"]</rosparam>
|
||||
</node>
|
||||
<!-- Combine joint values to TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>
|
||||
<!--KDL chain related args-->
|
||||
<param if="$(arg load_robot_description)" name="$(arg urdf_param)" command="$(find xacro)/xacro --inorder $(find kair_algorithms)/urdf/om.urdf"/>
|
||||
|
||||
<!-- Show in Rviz -->
|
||||
<group if="$(arg open_rviz)">
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find open_manipulator_description)/rviz/open_manipulator.rviz"/>
|
||||
</group>
|
||||
|
||||
<!-- Load the URDF into the ROS Parameter Server -->
|
||||
<!-- Load the URDF into the ROS Parameter Server -->
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro --inorder '$(find open_manipulator_description)/urdf/open_manipulator.urdf.xacro'"/>
|
||||
|
||||
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
|
||||
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
|
||||
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
|
||||
args="-urdf -model open_manipulator -z 0.0 -param robot_description"/>
|
||||
|
||||
<!-- ros_control robotis manipulator launch file -->
|
||||
<!-- ros_control robotis manipulator launch file -->
|
||||
<include file="$(find open_manipulator_gazebo)/launch/open_manipulator_controller.launch"/>
|
||||
</launch>
|
||||
|
||||
@@ -1,21 +1,23 @@
|
||||
# ! usr/bin/env python
|
||||
|
||||
import time
|
||||
from abc import ABCMeta
|
||||
from math import cos, sin
|
||||
import time
|
||||
|
||||
import gym
|
||||
import numpy as np
|
||||
import rospkg # noqa
|
||||
|
||||
import rospy # noqa
|
||||
import tf # noqa
|
||||
import tf.transformations as tr # noqa
|
||||
from gazebo_msgs.srv import DeleteModel, GetModelState, SpawnModel
|
||||
from gazebo_msgs.srv import DeleteModel, GetModelState, SpawnModel # noqa
|
||||
from geometry_msgs.msg import Pose
|
||||
from open_manipulator_msgs.msg import KinematicsPose, OpenManipulatorState
|
||||
from pykdl_utils.kdl_kinematics import KDLKinematics
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Float64
|
||||
from urdf_parser_py.urdf import URDF # noqa
|
||||
|
||||
import rospkg # noqa
|
||||
|
||||
|
||||
class OpenManipulatorRosBaseInterface(object):
|
||||
@@ -37,6 +39,7 @@ class OpenManipulatorRosBaseInterface(object):
|
||||
self.termination_count = 0
|
||||
self.success_count = 0
|
||||
|
||||
self.init_fk_solver()
|
||||
self.init_tf_transformer()
|
||||
self.init_publish_node()
|
||||
self.init_subscribe_node()
|
||||
@@ -116,6 +119,10 @@ class OpenManipulatorRosBaseInterface(object):
|
||||
self.pub_joint3_position.publish(np.random.uniform(0.0, 0.0))
|
||||
self.pub_joint4_position.publish(np.random.uniform(0.0, 0.0))
|
||||
|
||||
def init_fk_solver(self):
|
||||
self.robot = URDF.from_parameter_server()
|
||||
self.solve_fk = KDLKinematics(self.robot, "world", "end_effector_link")
|
||||
|
||||
def joint_state_callback(self, msg):
|
||||
"""Callback function of joint states subscriber.
|
||||
|
||||
@@ -129,19 +136,20 @@ class OpenManipulatorRosBaseInterface(object):
|
||||
self.joint_efforts = joints_states.effort
|
||||
# penalize jerky motion in reward for shaped reward setting.
|
||||
self.squared_sum_vel = np.linalg.norm(np.array(self.joint_velocities))
|
||||
try:
|
||||
(
|
||||
self._gripper_position,
|
||||
self._gripper_orientation,
|
||||
) = self.tf_listenser.lookupTransform(
|
||||
"/world", "/end_effector_link", rospy.Time(0)
|
||||
)
|
||||
except (
|
||||
tf.LookupException,
|
||||
tf.ConnectivityException,
|
||||
tf.ExtrapolationException,
|
||||
):
|
||||
pass
|
||||
_fk_mat = np.array(self.solve_fk.forward(self.joint_positions[2:]))
|
||||
self._gripper_position = _fk_mat[0:3, 3]
|
||||
self._gripper_orientation[3] = (
|
||||
1 + _fk_mat[0, 0] + _fk_mat[1, 1] + _fk_mat[2, 2]
|
||||
) ** 0.5
|
||||
self._gripper_orientation[0] = (_fk_mat[2, 1] - _fk_mat[1, 2]) / (
|
||||
4 * self._gripper_orientation[3]
|
||||
)
|
||||
self._gripper_orientation[1] = (_fk_mat[0, 2] - _fk_mat[2, 0]) / (
|
||||
4 * self._gripper_orientation[3]
|
||||
)
|
||||
self._gripper_orientation[2] = (_fk_mat[1, 0] - _fk_mat[0, 1]) / (
|
||||
4 * self._gripper_orientation[3]
|
||||
)
|
||||
|
||||
def kinematics_pose_callback(self, msg):
|
||||
"""Callback function of gripper kinematic pose subscriber.
|
||||
@@ -378,7 +386,7 @@ class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface):
|
||||
if block_pose is not None:
|
||||
assert self.train_mode is True
|
||||
|
||||
# self.delete_target_block()
|
||||
# self.delete_target_block()
|
||||
self.init_robot_pose()
|
||||
time.sleep(0.5)
|
||||
|
||||
@@ -395,7 +403,7 @@ class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface):
|
||||
self.cfg["OVERHEAD_ORIENTATION"],
|
||||
)
|
||||
|
||||
# block_pose = Pose()
|
||||
# block_pose = Pose()
|
||||
block_pose_position_x = polar_rad * cos(polar_theta)
|
||||
block_pose_position_y = polar_rad * sin(polar_theta)
|
||||
block_pose_position_z = z
|
||||
@@ -408,19 +416,19 @@ class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface):
|
||||
|
||||
# TODO: Add block generation condition when testing gazebo simulation.
|
||||
|
||||
# block_reference_frame = "world"
|
||||
# model_path = rospkg.RosPack().get_path("kair_algorithms") + "/urdf/"
|
||||
#
|
||||
# with open(model_path + "block/model.urdf", "r") as block_file:
|
||||
# block_xml = block_file.read().replace("\n", "")
|
||||
#
|
||||
# rospy.wait_for_service("/gazebo/spawn_urdf_model")
|
||||
#
|
||||
# try:
|
||||
# spawn_urdf = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
|
||||
# spawn_urdf("block", block_xml, "/", block_pose, block_reference_frame)
|
||||
# except rospy.ServiceException as e:
|
||||
# rospy.logerr("Spawn URDF service call failed: {0}".format(e))
|
||||
# block_reference_frame = "world"
|
||||
# model_path = rospkg.RosPack().get_path("kair_algorithms") + "/urdf/"
|
||||
|
||||
# with open(model_path + "block/model.urdf", "r") as block_file:
|
||||
# block_xml = block_file.read().replace("\n", "")
|
||||
|
||||
# rospy.wait_for_service("/gazebo/spawn_urdf_model")
|
||||
|
||||
# try:
|
||||
# spawn_urdf = rospy.ServiceProxy("/gazebo/spawn_urdf_model", SpawnModel)
|
||||
# spawn_urdf("block", block_xml, "/", block_pose, block_reference_frame)
|
||||
# except rospy.ServiceException as e:
|
||||
# rospy.logerr("Spawn URDF service call failed: {0}".format(e))
|
||||
|
||||
def delete_target_block(self):
|
||||
"""This will be called on ROS Exit, deleting Gazebo models.
|
||||
@@ -441,22 +449,22 @@ class OpenManipulatorRosGazeboInterface(OpenManipulatorRosBaseInterface):
|
||||
Returns:
|
||||
L2 norm of end effector pose and object pose.
|
||||
"""
|
||||
# rospy.wait_for_service("/gazebo/get_model_state")
|
||||
#
|
||||
# try:
|
||||
# object_state_srv = rospy.ServiceProxy(
|
||||
# "/gazebo/get_model_state", GetModelState
|
||||
# )
|
||||
# object_state = object_state_srv("block", "world")
|
||||
# object_pose = [
|
||||
# object_state.pose.position.x,
|
||||
# object_state.pose.position.y,
|
||||
# object_state.pose.position.z,
|
||||
# ]
|
||||
# self._obj_pose = np.array(object_pose)
|
||||
# except rospy.ServiceException as e:
|
||||
# rospy.logerr("Spawn URDF service call failed: {0}".format(e))
|
||||
#
|
||||
# rospy.wait_for_service("/gazebo/get_model_state")
|
||||
|
||||
# try:
|
||||
# object_state_srv = rospy.ServiceProxy(
|
||||
# "/gazebo/get_model_state", GetModelState
|
||||
# )
|
||||
# object_state = object_state_srv("block", "world")
|
||||
# object_pose = [
|
||||
# object_state.pose.position.x,
|
||||
# object_state.pose.position.y,
|
||||
# object_state.pose.position.z,
|
||||
# ]
|
||||
# self._obj_pose = np.array(object_pose)
|
||||
# except rospy.ServiceException as e:
|
||||
# rospy.logerr("Spawn URDF service call failed: {0}".format(e))
|
||||
|
||||
# FK state of robot
|
||||
end_effector_pose = np.array(self._gripper_position)
|
||||
return np.linalg.norm(end_effector_pose - self.block_pose)
|
||||
|
||||
Executable
+306
@@ -0,0 +1,306 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="open_manipulator" xmlns:xacro="http://ros.org/wiki/xacro">
|
||||
<material name="black">
|
||||
<color rgba="0.0 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||
</material>
|
||||
<material name="red">
|
||||
<color rgba="0.8 0.0 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="blue">
|
||||
<color rgba="0.0 0.0 0.8 1.0"/>
|
||||
</material>
|
||||
<material name="green">
|
||||
<color rgba="0.0 0.8 0.0 1.0"/>
|
||||
</material>
|
||||
<material name="grey">
|
||||
<color rgba="0.5 0.5 0.5 1.0"/>
|
||||
</material>
|
||||
<material name="orange">
|
||||
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
|
||||
</material>
|
||||
<material name="brown">
|
||||
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
|
||||
</material>
|
||||
<!-- World -->
|
||||
<link name="world">
|
||||
</link>
|
||||
<!-- World fixed joint-->
|
||||
<joint name="world_fixed" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<parent link="world"/>
|
||||
<child link="link1"/>
|
||||
</joint>
|
||||
<!-- Link 1 -->
|
||||
<link name="link1">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link1.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<!-- <inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.082" />
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0"
|
||||
iyy="0.1" iyz="0.0"
|
||||
izz="0.1" />
|
||||
</inertial>-->
|
||||
<inertial>
|
||||
<origin xyz="3.0876154e-04 0.0000000e+00 -1.2176461e-04"/>
|
||||
<mass value="7.9119962e-02"/>
|
||||
<inertia ixx="1.2505234e-05" ixy="0.0" ixz="-1.7855208e-07" iyy="2.1898364e-05" iyz="0.0" izz="1.9267361e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- Joint 1 -->
|
||||
<joint name="joint1" type="revolute">
|
||||
<parent link="link1"/>
|
||||
<child link="link2"/>
|
||||
<origin rpy="0 0 0" xyz="0.012 0.0 0.017"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1" lower="-2.82743338823" upper="2.82743338823" velocity="4.8"/>
|
||||
</joint>
|
||||
<!-- Link 2 -->
|
||||
<link name="link2">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.019"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.019"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link2.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<!-- <inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.098" />
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0"
|
||||
iyy="0.1" iyz="0.0"
|
||||
izz="0.1" />
|
||||
</inertial>-->
|
||||
<inertial>
|
||||
<origin xyz="-3.0184870e-04 5.4043684e-04 0.047433464"/>
|
||||
<mass value="9.8406837e-02"/>
|
||||
<inertia ixx="3.4543422e-05" ixy="-1.6031095e-08" ixz="-3.8375155e-07" iyy="3.2689329e-05" iyz="2.8511935e-08" izz="1.8850320e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- Joint 2 -->
|
||||
<joint name="joint2" type="revolute">
|
||||
<parent link="link2"/>
|
||||
<child link="link3"/>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0595"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="1" lower="-1.79070781255" upper="1.57079632679" velocity="4.8"/>
|
||||
</joint>
|
||||
<!-- Link 3 -->
|
||||
<link name="link3">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link3.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<!-- <inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.136" />
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0"
|
||||
iyy="0.1" iyz="0.0"
|
||||
izz="0.1" />
|
||||
</inertial>-->
|
||||
<inertial>
|
||||
<origin xyz="1.0308393e-02 3.7743363e-04 1.0170197e-01"/>
|
||||
<mass value="1.3850917e-01"/>
|
||||
<inertia ixx="3.3055381e-04" ixy="-9.7940978e-08" ixz="-3.8505711e-05" iyy="3.4290447e-04" iyz="-1.5717516e-06" izz="6.0346498e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- Joint 3 -->
|
||||
<joint name="joint3" type="revolute">
|
||||
<parent link="link3"/>
|
||||
<child link="link4"/>
|
||||
<origin rpy="0 0 0" xyz="0.024 0 0.128"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="1" lower="-0.942477796077" upper="1.38230076758" velocity="4.8"/>
|
||||
</joint>
|
||||
|
||||
<!-- Link 4 -->
|
||||
<link name="link4">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link4.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<!-- <inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.131" />
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0"
|
||||
iyy="0.1" iyz="0.0"
|
||||
izz="0.1" />
|
||||
</inertial>-->
|
||||
<inertial>
|
||||
<origin xyz="9.0909590e-02 3.8929816e-04 2.2413279e-04"/>
|
||||
<mass value="1.3274562e-01"/>
|
||||
<inertia ixx="3.0654178e-05" ixy="-1.2764155e-06" ixz="-2.6874417e-07" iyy="2.4230292e-04" iyz="1.1559550e-08" izz="2.5155057e-04"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- Joint 4 -->
|
||||
<joint name="joint4" type="revolute">
|
||||
<parent link="link4"/>
|
||||
<child link="link5"/>
|
||||
<origin rpy="0 0 0" xyz="0.124 0.0 0.0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="1" lower="-1.79070781255" upper="2.04203522483" velocity="4.8"/>
|
||||
</joint>
|
||||
|
||||
<!-- Link 5 -->
|
||||
<link name="link5">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link5.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<!-- <inertial>
|
||||
<origin xyz="0 0 0" />
|
||||
<mass value="0.141" />
|
||||
<inertia ixx="0.1" ixy="0.0" ixz="0.0"
|
||||
iyy="0.1" iyz="0.0"
|
||||
izz="0.1" />
|
||||
</inertial>-->
|
||||
<inertial>
|
||||
<origin xyz="4.4206755e-02 3.6839985e-07 8.9142216e-03"/>
|
||||
<mass value="1.4327573e-01"/>
|
||||
<inertia ixx="8.0870749e-05" ixy="0.0" ixz="-1.0157896e-06" iyy="7.5980465e-05" iyz="0.0" izz="9.3127351e-05"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<!-- Gripper link -->
|
||||
<link name="gripper_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0"/>
|
||||
<mass value="0.017"/>
|
||||
<inertia ixx="1.0e-03" ixy="0.0" ixz="0.0" iyy="1.0e-03" iyz="0.0" izz="1.0e-03"/>
|
||||
</inertial>
|
||||
<!-- <inertial>
|
||||
<origin xyz="${0.028 + 8.3720668e-03} ${0.0246 + 9.9696160e-03} -4.2836895e-07" />
|
||||
<mass value="3.2218127e-02" />
|
||||
<inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
|
||||
iyy="2.2552871e-05" iyz="-3.1463634e-10"
|
||||
izz="1.7605306e-05" />
|
||||
</inertial>-->
|
||||
</link>
|
||||
<!-- Gripper joint -->
|
||||
<joint name="gripper" type="prismatic">
|
||||
<parent link="link5"/>
|
||||
<child link="gripper_link"/>
|
||||
<origin rpy="0 0 0" xyz="0.0817 0.021 0.0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit effort="1" lower="-0.010" upper="0.019" velocity="4.8"/>
|
||||
</joint>
|
||||
|
||||
<!-- Gripper link sub -->
|
||||
<link name="gripper_link_sub">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0.0 -0.0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.0 -0.0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://open_manipulator_description/meshes/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0"/>
|
||||
<mass value="0.017"/>
|
||||
<inertia ixx="1.0e-03" ixy="0.0" ixz="0.0" iyy="1.0e-03" iyz="0.0" izz="1.0e-03"/>
|
||||
</inertial>
|
||||
<!-- <inertial>
|
||||
<origin xyz="${0.028 + 8.3720668e-03} ${-0.0246 - 9.9696160e-03} -4.2836895e-07" />
|
||||
<mass value="3.2218127e-02" />
|
||||
<inertia ixx="9.5568826e-06" ixy="2.8424644e-06" ixz="-3.2829197e-10"
|
||||
iyy="2.2552871e-05" iyz="-3.1463634e-10"
|
||||
izz="1.7605306e-05" />
|
||||
</inertial>-->
|
||||
</link>
|
||||
<!-- Gripper joint sub -->
|
||||
<joint name="gripper_sub" type="prismatic">
|
||||
<parent link="link5"/>
|
||||
<child link="gripper_link_sub"/>
|
||||
<origin rpy="0 0 0" xyz="0.0817 -0.021 0"/>
|
||||
<axis xyz="0 -1 0"/>
|
||||
<limit effort="1" lower="-0.010" upper="0.019" velocity="4.8"/>
|
||||
<mimic joint="gripper" multiplier="1"/>
|
||||
</joint>
|
||||
|
||||
<!-- end effector joint -->
|
||||
<joint name="end_effector_joint" type="fixed">
|
||||
<origin rpy="0 0 0" xyz="0.126 0.0 0.0"/>
|
||||
<parent link="link5"/>
|
||||
<child link="end_effector_link"/>
|
||||
</joint>
|
||||
<!-- end effector link -->
|
||||
<link name="end_effector_link">
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.01 0.01 0.01"/>
|
||||
</geometry>
|
||||
<material name="red"/>
|
||||
</visual>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
|
||||
<mass value="0.001"/>
|
||||
<inertia ixx="1.0e-06" ixy="0.0" ixz="0.0" iyy="1.0e-06" iyz="0.0" izz="1.0e-06"/>
|
||||
</inertial>
|
||||
</link>
|
||||
</robot>
|
||||
Reference in New Issue
Block a user