Skip to content

Commit

Permalink
Added test folder for bindings
Browse files Browse the repository at this point in the history
  • Loading branch information
@EnricoMingo
EnricoMingo committed May 9, 2024
1 parent f497579 commit c0c1313
Show file tree
Hide file tree
Showing 6 changed files with 723 additions and 0 deletions.
313 changes: 313 additions & 0 deletions bindings/python/tests/panda.urdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,313 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from panda.urdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="panda">
<link name="panda_link0">
<visual>
<geometry>
<mesh filename="package://franka_description/meshes/visual/link0.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="package://franka_description/meshes/visual/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="package://franka_description/meshes/visual/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="package://franka_description/meshes/visual/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="package://franka_description/meshes/visual/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="package://franka_description/meshes/visual/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="package://franka_description/meshes/visual/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="package://franka_description/meshes/visual/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="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.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>
<link name="panda_link8">
<collision>
<origin rpy="3.141592653589793 1.5707963267948966 1.5707963267948966" xyz="0.0424 0.0424 -0.0250"/>
<geometry>
<cylinder length="0.01" radius="0.06"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.0424 0.0424 -0.02"/>
<geometry>
<sphere radius="0.06"/>
</geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.0424 0.0424 -0.03"/>
<geometry>
<sphere radius="0.06"/>
</geometry>
</collision>
</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"/>
</joint>
<link name="world"/>
<joint name="world_joint" type="fixed">
<parent link="world"/>
<child link="panda_link0"/>
</joint>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
</robot>

81 changes: 81 additions & 0 deletions bindings/python/tests/pyopensot_aggregated_tests.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
from pyopensot import GenericTask, GenericConstraint, ConstraintType, AggregatedTask, AggregatedConstraint, AffineHelper, AggregationPolicy, SubConstraint
import numpy as np
import unittest

M1 = np.array([[1, 2, 3]])
q1 = np.array([0])

c1 = GenericConstraint("c1",
AffineHelper(M1, q1),
np.array([1]), np.array([-1]),
ConstraintType.CONSTRAINT)
c1.update()

M2 = np.array([[4, 5, 6]])
q2 = np.array([0])

c2 = GenericConstraint("c2",
AffineHelper(M2, q2),
2*np.array([1]), 2*np.array([-1]),
ConstraintType.CONSTRAINT)
c2.update()

c3 = AggregatedConstraint(c1, c2, 3)

print(f"c3.getAineq(): {c3.getAineq()}")
print(f"c3.getbLowerBound(): {c3.getbLowerBound()}")
print(f"c3.getbUpperBound(): {c3.getbUpperBound()}")

c1.setBounds(4*np.array([1]), 4*np.array([-1]))
c3.update()
print(f"c3.getbLowerBound(): {c3.getbLowerBound()}")
print(f"c3.getbUpperBound(): {c3.getbUpperBound()}")

clist = []
clist.append(c1)
clist.append(c2)
clist.append(c3)
c4 = AggregatedConstraint(clist, 3)
print(f"c4.getAineq(): {c4.getAineq()}")
print(f"c4.getbLowerBound(): {c4.getbLowerBound()}")
print(f"c4.getbUpperBound(): {c4.getbUpperBound()}")

constraints = c4.getConstraintsList()
for c in constraints:
print(c.getConstraintID())

t1 = GenericTask("t1", np.array([[1,2,3],[4,5,6]]), np.array([[1],[2]]))
t2 = GenericTask("t2", np.array([[7,8,9],[10,11,12], [13,14,15]]), np.array([[3],[4],[5]]))
t3 = AggregatedTask(t1,t2,3)
t3.update()
print(f"t3.getA(): {t3.getA()}")
print(f"t3.getb(): {t3.getb()}")
t2.setAb(0.*np.array([[7,8,9],[10,11,12], [13,14,15]]), 0.*np.array([[3],[4],[5]]))
t3.update()
print(f"t3.getA(): {t3.getA()}")
print(f"t3.getb(): {t3.getb()}")
tlist = []
tlist.append(t1)
tlist.append(t2)
tlist.append(t3)
t4 = AggregatedTask(tlist, 3)
tasks = t4.getTaskList()
for t in tasks:
print(t.getTaskID())


print(f"c4.getA(): {c4.getAineq()}")
print(f"c4.getbUpperBound(): {c4.getbUpperBound()}")
print(f"c4.getbLowerBound(): {c4.getbLowerBound()}")
idx = [0, 2, 3]
sc4 = SubConstraint(c4, idx)
print(f"sc4.getAineq(): {sc4.getAineq()}")
print(f"sc4.getbUpperBound(): {sc4.getbUpperBound()}")
print(f"sc4.getbLowerBound(): {sc4.getbLowerBound()}")

c1.setBounds(np.array([100]), np.array([-100]))
sc4.update()
print(f"sc4.getAineq(): {sc4.getAineq()}")
print(f"sc4.getbUpperBound(): {sc4.getbUpperBound()}")
print(f"sc4.getbLowerBound(): {sc4.getbLowerBound()}")

Loading

0 comments on commit c0c1313

Please sign in to comment.