Skip to content

Commit

Permalink
added truncated and regular panda
Browse files Browse the repository at this point in the history
  • Loading branch information
@filipmrc
filipmrc committed Jun 28, 2022
1 parent c2d0538 commit c9cf92b
Show file tree
Hide file tree
Showing 5 changed files with 366 additions and 46 deletions.
287 changes: 287 additions & 0 deletions graphik/robots/urdfs/panda_arm_truncated.urdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,287 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from panda_arm.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="panda">
<link name="world" />
<joint name="world_joint" type="fixed">
<parent link="world"/>
<child link="panda_link0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
</joint>
<link name="panda_link0">
<visual>
<geometry>
<mesh filename="meshes/panda_arm/link0_resaved.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 1.5707963267948966 0" xyz="-0.075 0 0.06"/>
<geometry>
<cylinder length="0.03" radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.06 0 0.06"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.09 0 0.06"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</collision>
</link>
<link name="panda_link1">
<visual>
<geometry>
<mesh filename="meshes/panda_arm/link1.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.1915"/>
<geometry>
<cylinder length="0.2830" radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.333"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.05"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</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.8973" upper="2.8973" velocity="2.1750"/>
</joint>
<link name="panda_link2">
<visual>
<geometry>
<mesh filename="meshes/panda_arm/link2.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.12" radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.06"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.06"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</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.5707963267948966 0 0" xyz="0 0 0"/>
<parent link="panda_link1"/>
<child link="panda_link2"/>
<axis xyz="0 0 1"/>
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
</joint>
<link name="panda_link3">
<visual>
<geometry>
<mesh filename="meshes/panda_arm/link3.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.145"/>
<geometry>
<cylinder length="0.15" radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.22"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.07"/>
<geometry>
<sphere radius="0.09"/>
</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.5707963267948966 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.8973" upper="2.8973" velocity="2.1750"/>
</joint>
<link name="panda_link4">
<visual>
<geometry>
<mesh filename="meshes/panda_arm/link4.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.12" radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.06"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.06"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</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.5707963267948966 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.0718" upper="-0.0698" velocity="2.1750"/>
</joint>
<link name="panda_link5">
<visual>
<geometry>
<mesh filename="meshes/panda_arm/link5.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.26"/>
<geometry>
<cylinder length="0.1" radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.31"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.21"/>
<geometry>
<sphere radius="0.09"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0.08 -0.13"/>
<geometry>
<cylinder length="0.14" radius="0.055"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0.08 -0.06"/>
<geometry>
<sphere radius="0.055"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0.08 -0.20"/>
<geometry>
<sphere radius="0.055"/>
</geometry>
</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.5707963267948966 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.8973" upper="2.8973" velocity="2.6100"/>
</joint>
<link name="panda_link6">
<visual>
<geometry>
<mesh filename="meshes/panda_arm/link6.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
<geometry>
<cylinder length="0.08" radius="0.08"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.01"/>
<geometry>
<sphere radius="0.08"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.07"/>
<geometry>
<sphere radius="0.08"/>
</geometry>
</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.5707963267948966 0 0" xyz="0 0 0"/>
<parent link="panda_link5"/>
<child link="panda_link6"/>
<axis xyz="0 0 1"/>
<limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
</joint>
<link name="panda_link7">
<visual>
<geometry>
<mesh filename="meshes/panda_arm/link7.dae"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.01"/>
<geometry>
<cylinder length="0.14" radius="0.07"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.08"/>
<geometry>
<sphere radius="0.07"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.06"/>
<geometry>
<sphere radius="0.07"/>
</geometry>
</collision>
</link>
<joint name="panda_joint7" type="fixed">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="1.5707963267948966 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.8973" upper="2.8973" velocity="2.6100"/>
</joint>
</robot>
11 changes: 11 additions & 0 deletions graphik/utils/dgp.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -229,3 +229,14 @@ def bound_smoothing(G: nx.DiGraph) -> tuple:
upper_bounds[ids.index(u), ids.index(v)] = bounds[u][v]

return lower_bounds, upper_bounds

def normalize_positions(Y: ArrayLike, scale=False):
Y_c = Y - Y.mean(0)
C = Y_c.T.dot(Y_c)
e, v = np.linalg.eig(C)
Y_cr = Y_c.dot(v)
if scale:
Y_crs = Y_cr / (1 / abs(Y_cr).max())
return Y_crs
else:
return Y_cr
13 changes: 13 additions & 0 deletions graphik/utils/roboturdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -579,6 +579,19 @@ def load_truncated_ur10(n: int):
graph = ProblemGraphRevolute(robot)
return robot, graph

def load_panda(limits=None):
fname = graphik.__path__[0] + "/robots/urdfs/panda_arm.urdf"
urdf_robot = RobotURDF(fname)
n = urdf_robot.n_q_joints
if limits is None:
ub = np.ones(n) * np.pi
lb = -ub
else:
lb = limits[0]
ub = limits[1]
robot = urdf_robot.make_Revolute3d(ub, lb) # make the Revolute class from a URDF
graph = ProblemGraphRevolute(robot)
return robot, graph

def load_9_dof(limits=None):
"""
Expand Down
2 changes: 1 addition & 1 deletion setup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
url="https://github.com/utiasSTARS/graphIK",
packages=find_packages(),
install_requires=[
"numpy >= 1.16",
"numpy == 1.20",
"scipy >= 1.3.0",
"sympy >= 1.5",
"matplotlib >= 3.1",
Expand Down
Loading

0 comments on commit c9cf92b

Please sign in to comment.