Commit ac7618f2 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

collision object for the print head (implement #22)

parent c481e799
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from ../../universal_robot/ur_e_description/urdf/ur5e_robot.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="ur5e">
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="ros_control">
<!--robotNamespace>/</robotNamespace-->
<!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
</plugin>
<!--
<plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
<alwaysOn>true</alwaysOn>
<updateRate>1.0</updateRate>
<timeout>5</timeout>
<powerStateTopic>power_state</powerStateTopic>
<powerStateRate>10.0</powerStateRate>
<fullChargeCapacity>87.78</fullChargeCapacity>
<dischargeRate>-474</dischargeRate>
<chargeRate>525</chargeRate>
<dischargeVoltage>15.52</dischargeVoltage>
<chargeVoltage>16.41</chargeVoltage>
</plugin>
-->
</gazebo>
<!--property name="shoulder_height" value="0.089159" /-->
<!--property name="shoulder_offset" value="0.13585" /-->
<!-- shoulder_offset - elbow_offset + wrist_1_length = 0.10915 -->
<!--property name="upper_arm_length" value="0.42500" /-->
<!--property name="elbow_offset" value="0.1197" /-->
<!-- CAD measured -->
<!--property name="forearm_length" value="0.39225" /-->
<!--property name="wrist_1_length" value="0.093" /-->
<!-- CAD measured -->
<!--property name="wrist_2_length" value="0.09465" /-->
<!-- In CAD this distance is 0.930, but in the spec it is 0.09465 -->
<!--property name="wrist_3_length" value="0.0823" /-->
<!-- manually measured -->
<link name="base_link">
<visual>
<geometry>
......@@ -331,6 +294,29 @@
<parent link="wrist_3_link"/>
<child link="tool0"/>
</joint>
<link name="print_tip">
<visual>
<origin rpy="0 0 0" xyz="0.075 0 0"/>
<geometry>
<box size="0.15 0.1 0.1"/>
</geometry>
<material name="pt_material">
<color rgba="0.3 0.8 0.3 0.95"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.075 0 0"/>
<geometry>
<box size="0.15 0.1 0.1"/>
</geometry>
</collision>
</link>
<joint name="pt_to_robot" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="ee_link"/>
<child link="print_tip"/>
</joint>
<link name="world"/>
<joint name="world_joint" type="fixed">
<parent link="world"/>
......
......@@ -10,9 +10,7 @@ home = [0, -2.1, 2.1, -pi/2, -pi/2, 0]
def test_pos(pos, env):
js = env.ik_pos(pos, 0)
env.setJoints(0, js[0])
p_fk= env._fk(js[0],0)[:3,3]
p_fk_tip = env.fk(js[0],0)[:3,3]
print(p_fk)
print(p_fk_tip)
if __name__ == "__main__":
......
......@@ -27,7 +27,7 @@ def defaultPlanningEnv():
return PlanningEnv([[0,0,1,0],[0,0,0,1]],
[[0.5, 0, 0.001],[-0.5, 0, 0.001]],
[[0,0,0,1],[0,0,0,1]],
[[0.1,0.01,0.05],[0.1,0.1,0.2]])
[[0.15,0,0],[0.15,0,0]]) # WARNING - also edit urdf
urdf_folder = Path(__file__).parent / "../../models/urdf"
......@@ -140,14 +140,6 @@ class SimEnv:
return np.asarray(js).reshape(n_sols,6)
def _fk(self, joints, robot):
"""
deprecated - does not include the print tip transform
"""
pmat = np.asarray(self.ur5e_kin.forward(joints)).reshape(3,4)
return np.linalg.inv(self.world2armT[robot]) @ np.concatenate((pmat, [[0,0,0,1]]))
def fk(self, joints, robot):
pmat = np.asarray(self.ur5e_kin.forward(joints)).reshape(3,4)
return np.linalg.inv(self.world2armT[robot]) @ \
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment