def test_jacobm(self):
panda = rp.Panda()
q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
q3 = np.expand_dims(q1, 0)
q4 = np.expand_dims(q1, 1)
ans = np.array([[1.27080875e-17], [2.38242538e-02], [6.61029519e-03],
[8.18202121e-03], [7.74546204e-04], [-1.10885380e-02],
[0.00000000e+00]])
panda.q = q1
nt.assert_array_almost_equal(panda.jacobm(), ans)
nt.assert_array_almost_equal(panda.jacobm(q2), ans)
nt.assert_array_almost_equal(panda.jacobm(q3), ans)
nt.assert_array_almost_equal(panda.jacobm(q4), ans)
nt.assert_array_almost_equal(panda.jacobm(J=panda.jacob0(q1)), ans)
# self.assertRaises(ValueError, panda.jacobm)
self.assertRaises(TypeError, panda.jacobm, 'Wfgsrth')
self.assertRaises(ValueError, panda.jacobm, [1, 3], np.array([1, 5]))
self.assertRaises(TypeError, panda.jacobm, [1, 3], 'qwe')
self.assertRaises(TypeError, panda.jacobm, [1, 3], panda.jacob0(q1),
[1, 2, 3])
self.assertRaises(ValueError, panda.jacobm, [1, 3], panda.jacob0(q1),
np.array([1, 2, 3]))
def __init__(self):
super(GraspEnv, self).__init__()
# Initialize rospy node
rospy.init_node('grasp_env', anonymous=True)
# Initialize service from cameraEnv
service_name = '/camera_env/infer_grasp'
rospy.wait_for_service(service_name)
self.grasp_infer_srv = rospy.ServiceProxy(service_name, GraspInfer)
# Set up panda moveit commander, pose control
self.pc = PandaCommander(group_name='panda_arm')
# Set up ropy and joint velocity controller
self.panda = rp.Panda()
self.joint_velocity_pub = rospy.Publisher('/cartesian_velocity_controller/cartesian_velocity', Twist, queue_size=1)
# Subscribe to robot state
self.robot_state = None
rospy.Subscriber('/franka_state_controller/franka_states', FrankaState, self.__robot_state_callback, queue_size=1)
# For visualization in Rviz
self.markers = RvizMarkers('/panda_link0', 'visualization_marker')
# Errors
self.ROBOT_ERROR_DETECTED = False
# Add table as collision
self.pc.add_table()
def test_getters(self):
panda = rp.Panda()
panda.qdd = np.ones((7, 1))
panda.qd = np.ones((1, 7))
panda.qdd = panda.qd
panda.qd = panda.qdd
def test_control_type2(self):
panda = rp.Panda()
panda.control_type = 'p'
with self.assertRaises(ValueError):
panda.control_type = 'z'
def test_jacob0v(self):
pdh = rp.PandaMDH()
panda = rp.Panda()
q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
panda.q = q1
nt.assert_array_almost_equal(panda.jacob0v(), pdh.jacob0v(q1))
def get_cartesian_manipulability_cb(self, req):
try:
panda = rp.Panda()
panda.q = np.array(
self.moveit_commander.get_pose_ik(req.stamped_pose))
return panda.m
except:
return 0
def test_panda(self):
panda = rp.Panda()
qz = np.array([0, 0, 0, 0, 0, 0, 0])
qr = panda.qr
nt.assert_array_almost_equal(panda.qr, qr)
nt.assert_array_almost_equal(panda.qz, qz)
nt.assert_array_almost_equal(panda.gravity, np.array([[0], [0],
[9.81]]))
def test_base(self):
panda = rp.Panda()
pose = sm.SE3()
panda.base = pose.A
nt.assert_array_almost_equal(np.eye(4), panda.base.A)
panda.base = pose
nt.assert_array_almost_equal(np.eye(4), panda.base.A)
def test_q(self):
panda = rp.Panda()
q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
q3 = np.expand_dims(q1, 0)
panda.q = q1
nt.assert_array_almost_equal(panda.q, q1)
panda.q = q2
nt.assert_array_almost_equal(panda.q, q2)
panda.q = q3
nt.assert_array_almost_equal(np.expand_dims(panda.q, 0), q3)
def test_fkine_traj(self):
panda = rp.Panda()
q = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
qq = np.c_[q, q, q, q]
ans = np.array([[-0.50827907, -0.57904589, 0.63746234, 0.44682295],
[0.83014553, -0.52639462, 0.18375824, 0.16168396],
[0.22915229, 0.62258699, 0.74824773, 0.96798113],
[0., 0., 0., 1.]])
TT = panda.fkine(qq)
nt.assert_array_almost_equal(TT[0].A, ans)
nt.assert_array_almost_equal(TT[1].A, ans)
nt.assert_array_almost_equal(TT[2].A, ans)
nt.assert_array_almost_equal(TT[3].A, ans)
def test_allfkine(self):
pm = rp.PandaMDH()
p = rp.Panda()
q = [1, 2, 3, 4, 5, 6, 7]
p.q = q
pm.q = q
p.allfkine()
r2 = pm.allfkine()
for i in range(7):
nt.assert_array_almost_equal(p.ets[i]._fk.A, r2[i].A)
p.allfkine(q)
for i in range(7):
nt.assert_array_almost_equal(p.ets[i]._fk.A, r2[i].A)
def test_fkine(self):
panda = rp.Panda()
q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
q3 = np.expand_dims(q1, 0)
ans = np.array([[-0.50827907, -0.57904589, 0.63746234, 0.44682295],
[0.83014553, -0.52639462, 0.18375824, 0.16168396],
[0.22915229, 0.62258699, 0.74824773, 0.96798113],
[0., 0., 0., 1.]])
panda.q = q1
nt.assert_array_almost_equal(panda.fkine().A, ans)
nt.assert_array_almost_equal(panda.fkine(q2).A, ans)
nt.assert_array_almost_equal(panda.fkine(q3).A, ans)
nt.assert_array_almost_equal(panda.fkine(q3).A, ans)
self.assertRaises(TypeError, panda.fkine, 'Wfgsrth')
def __init__(self):
super(PushEnv, self).__init__()
# Initialize rospy node
rospy.init_node('grasp_env', anonymous=True)
# Set up panda moveit commander, pose control
self.pc = PandaCommander(group_name='panda_arm')
# Initialize
rospy.Subscriber('/push_infer', Vector3, self.__push_infer_callback, queue_size=1)
self.delta_x = 0.0
self.delta_y = 0.0
self.delta_x_buffer = 0.0
self.delta_y_buffer = 0.0
# Set up ropy and joint velocity controller
self.panda = rp.Panda()
self.curr_velocity_publish_rate = 100.0 # for libfranka
self.curr_velo_pub = rospy.Publisher('/joint_velocity_node_controller/joint_velocity', Float64MultiArray, queue_size=1)
self.curr_velo = Float64MultiArray()
# Set up switch between moveit and velocity, start with moveit
self.cs = ControlSwitcher({
'moveit': 'position_joint_trajectory_controller',
'velocity': 'joint_velocity_node_controller'})
self.cs.switch_controller('moveit')
# Set up update with inference
self.update_rate = 5.0 # for inference
update_topic_name = '~/update'
self.update_pub = rospy.Publisher(update_topic_name, Empty, queue_size=1)
rospy.Subscriber(update_topic_name, Empty, self.__update_callback, queue_size=1)
# Subscribe to robot state
self.robot_state = None
rospy.Subscriber('/franka_state_controller/franka_states', FrankaState, self.__robot_state_callback, queue_size=1)
# For visualization in Rviz
self.markers = RvizMarkers('/panda_link0', 'visualization_marker')
# Errors
self.ROBOT_ERROR_DETECTED = False
self.BAD_UPDATE = False
def test_manipulability(self):
panda = rp.Panda()
q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
q3 = np.expand_dims(q1, 0)
q4 = np.expand_dims(q1, 1)
ans = 0.006559178039088341
panda.q = q1
nt.assert_array_almost_equal(panda.manipulability(), ans)
nt.assert_array_almost_equal(panda.manipulability(q2), ans)
nt.assert_array_almost_equal(panda.manipulability(q3), ans)
nt.assert_array_almost_equal(panda.manipulability(q4), ans)
# self.assertRaises(ValueError, panda.manipulability)
self.assertRaises(TypeError, panda.manipulability, 'Wfgsrth')
self.assertRaises(ValueError, panda.manipulability, [1, 3],
np.array([1, 5]))
self.assertRaises(TypeError, panda.manipulability, [1, 3], 'qwe')
def test_jacob0(self):
panda = rp.Panda()
q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
q3 = np.expand_dims(q1, 0)
q4 = np.expand_dims(q1, 1)
ans = np.array([[
-1.61683957e-01, 1.07925929e-01, -3.41453006e-02, 3.35029257e-01,
-1.07195463e-02, 1.03187865e-01, 0.00000000e+00
],
[
4.46822947e-01, 6.25741987e-01, 4.16474664e-01,
-8.04745724e-02, 7.78257566e-02, -1.17720983e-02,
0.00000000e+00
],
[
0.00000000e+00, -2.35276631e-01, -8.20187641e-02,
-5.14076923e-01, -9.98040745e-03, -2.02626953e-01,
0.00000000e+00
],
[
1.29458954e-16, -9.85449730e-01, 3.37672585e-02,
-6.16735653e-02, 6.68449878e-01, -1.35361558e-01,
6.37462344e-01
],
[
9.07021273e-18, 1.69967143e-01, 1.95778638e-01,
9.79165111e-01, 1.84470262e-01, 9.82748279e-01,
1.83758244e-01
],
[
1.00000000e+00, -2.26036604e-17, 9.80066578e-01,
-1.93473657e-01, 7.20517510e-01, -1.26028049e-01,
7.48247732e-01
]])
panda.q = q1
nt.assert_array_almost_equal(panda.jacob0(), ans)
nt.assert_array_almost_equal(panda.jacob0(q2), ans)
nt.assert_array_almost_equal(panda.jacob0(q3), ans)
nt.assert_array_almost_equal(panda.jacob0(q4), ans)
self.assertRaises(TypeError, panda.jacob0, 'Wfgsrth')
def __init__(self):
self.move_group = rospy.get_param('~move_group', None)
self.configuration = rp.Panda()
if not self.move_group:
rospy.logerr(
'Unable to load move_group name from rosparam server path: move_group'
)
sys.exit(1)
rospy.wait_for_service('/franka_control/set_EE_frame')
self.set_ee = rospy.ServiceProxy('/franka_control/set_EE_frame',
SetEEFrame)
rospy.wait_for_service('/franka_control/set_cartesian_impedance')
self.cartesian_impedance_proxy = rospy.ServiceProxy(
'/franka_control/set_cartesian_impedance',
FrankaSetCartesianImpedance)
ManipulationDriver.__init__(self,
PandaMoveItCommander(self.move_group))
self.velocity_publisher = rospy.Publisher(
'/cartesian_velocity_node_controller/cartesian_velocity',
Twist,
queue_size=1)
self.joint_velocity_publisher = rospy.Publisher(
'/joint_velocity_node_controller/joint_velocity',
JointVelocity,
queue_size=1)
self.recover_on_estop = rospy.get_param(
'/manipulation_commander/recover_on_estop', True)
# handling e-stop
rospy.Subscriber('/franka_state_controller/franka_states', FrankaState,
self.state_cb)
self.last_estop_state = 0
def get_joint_manipulability_cb(self, req):
panda = rp.Panda()
panda.q = np.array(req.joints)
return panda.m
def test_hessian0(self):
panda = rp.Panda()
q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9])
q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]
q3 = np.expand_dims(q1, 0)
q4 = np.expand_dims(q1, 1)
ans = np.array([
[[
-4.46822947e-01, -6.25741987e-01, -4.16474664e-01,
8.04745724e-02, -7.78257566e-02, 1.17720983e-02, 0.00000000e+00
],
[
-6.25741987e-01, -3.99892968e-02, -1.39404950e-02,
-8.73761859e-02, -1.69634134e-03, -3.44399243e-02,
0.00000000e+00
],
[
-4.16474664e-01, -1.39404950e-02, -4.24230421e-01,
-2.17748413e-02, -7.82283735e-02, -2.81325889e-02,
0.00000000e+00
],
[
8.04745724e-02, -8.73761859e-02, -2.17748413e-02,
-5.18935898e-01, 5.28476698e-03, -2.00682834e-01,
0.00000000e+00
],
[
-7.78257566e-02, -1.69634134e-03, -7.82283735e-02,
5.28476698e-03, -5.79159088e-02, -2.88966443e-02,
0.00000000e+00
],
[
1.17720983e-02, -3.44399243e-02, -2.81325889e-02,
-2.00682834e-01, -2.88966443e-02, -2.00614904e-01,
0.00000000e+00
],
[
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00
]],
[[
-1.61683957e-01, 1.07925929e-01, -3.41453006e-02,
3.35029257e-01, -1.07195463e-02, 1.03187865e-01, 0.00000000e+00
],
[
1.07925929e-01, -2.31853293e-01, -8.08253690e-02,
-5.06596965e-01, -9.83518983e-03, -1.99678676e-01,
0.00000000e+00
],
[
-3.41453006e-02, -8.08253690e-02, -3.06951191e-02,
3.45709946e-01, -1.01688580e-02, 1.07973135e-01,
0.00000000e+00
],
[
3.35029257e-01, -5.06596965e-01, 3.45709946e-01,
-9.65242924e-02, 1.45842251e-03, -3.24608603e-02,
0.00000000e+00
],
[
-1.07195463e-02, -9.83518983e-03, -1.01688580e-02,
1.45842251e-03, -1.05221866e-03, 2.09794626e-01,
0.00000000e+00
],
[
1.03187865e-01, -1.99678676e-01, 1.07973135e-01,
-3.24608603e-02, 2.09794626e-01, -4.04324654e-02,
0.00000000e+00
],
[
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00
]],
[[
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00
],
[
0.00000000e+00, -6.34981134e-01, -4.04611266e-01,
2.23596800e-02, -7.48714002e-02, -5.93773551e-03,
0.00000000e+00
],
[
0.00000000e+00, -4.04611266e-01, 2.07481281e-02,
-6.83089775e-02, 4.72662062e-03, -2.05994912e-02,
0.00000000e+00
],
[
0.00000000e+00, 2.23596800e-02, -6.83089775e-02,
-3.23085806e-01, 5.69641385e-03, -1.00311930e-01,
0.00000000e+00
],
[
0.00000000e+00, -7.48714002e-02, 4.72662062e-03,
5.69641385e-03, 5.40000550e-02, -2.69041502e-02,
0.00000000e+00
],
[
0.00000000e+00, -5.93773551e-03, -2.05994912e-02,
-1.00311930e-01, -2.69041502e-02, -9.98142073e-02,
0.00000000e+00
],
[
0.00000000e+00, 0.00000000e+00, 0.00000000e+00,
0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00
]],
[[
-9.07021273e-18, -2.77555756e-17, -2.77555756e-17,
-1.11022302e-16, -2.77555756e-17, 0.00000000e+00,
-2.77555756e-17
],
[
-1.69967143e-01, -1.97756387e-17, 4.11786040e-17,
-1.48932398e-16, -5.07612940e-17, -8.38219650e-17,
-4.90138154e-17
],
[
-1.95778638e-01, 1.66579116e-01, -1.38777878e-17,
1.04083409e-17, -1.38777878e-17, 3.46944695e-18,
0.00000000e+00
],
[
-9.79165111e-01, -3.28841647e-02, -9.97525009e-01,
-4.16333634e-17, -1.14491749e-16, 1.38777878e-17,
-6.24500451e-17
],
[
-1.84470262e-01, 1.22464303e-01, -3.97312016e-02,
7.41195745e-01, -2.77555756e-17, 1.12757026e-16,
2.77555756e-17
],
[
-9.82748279e-01, -2.14206274e-02, -9.87832342e-01,
6.67336352e-02, -7.31335770e-01, 2.08166817e-17,
-6.07153217e-17
],
[
-1.83758244e-01, 1.27177529e-01, -3.36043908e-02,
7.68210453e-01, 5.62842325e-03, 7.58497864e-01, 0.00000000e+00
]],
[[
1.29458954e-16, -1.11022302e-16, 8.67361738e-17,
-4.16333634e-17, 5.55111512e-17, 2.77555756e-17, 5.55111512e-17
],
[
-9.85449730e-01, -6.36381327e-17, -1.02735399e-16,
-1.83043043e-17, -5.63484308e-17, 8.08886307e-18,
1.07112702e-18
],
[
3.37672585e-02, 9.65806345e-01, 8.32667268e-17,
-2.55871713e-17, 1.07552856e-16, 2.08166817e-17,
-5.20417043e-18
],
[
-6.16735653e-02, -1.90658563e-01, -5.39111251e-02,
-6.59194921e-17, -2.77555756e-17, 2.38524478e-17,
-4.16333634e-17
],
[
6.68449878e-01, 7.10033786e-01, 6.30795483e-01,
-8.48905588e-02, 0.00000000e+00, 3.46944695e-17,
2.77555756e-17
],
[
-1.35361558e-01, -1.24194307e-01, -1.28407717e-01,
1.84162966e-02, -1.32869389e-02, 2.77555756e-17,
-2.08166817e-17
],
[
6.37462344e-01, 7.37360525e-01, 5.99489263e-01,
-7.71850655e-02, -4.08633244e-02, 2.09458434e-02,
0.00000000e+00
]],
[[
0.00000000e+00, -6.59521910e-17, -1.31033786e-16,
-1.92457571e-16, 1.54134782e-17, -7.69804929e-17,
1.11140361e-17
],
[
0.00000000e+00, -2.77555756e-17, 7.15573434e-17,
1.65666092e-16, 1.38777878e-17, -8.67361738e-18,
3.46944695e-17
],
[
0.00000000e+00, -1.98669331e-01, 8.67361738e-18,
-1.46584134e-16, 6.02816408e-17, -3.12250226e-17,
6.11490025e-17
],
[
0.00000000e+00, -9.54435515e-01, 4.51380881e-02,
1.38777878e-17, 1.08420217e-16, 3.46944695e-18, 6.24500451e-17
],
[
0.00000000e+00, -2.95400686e-01, -1.24639152e-01,
-6.65899738e-01, -4.85722573e-17, -5.20417043e-18,
-5.55111512e-17
],
[
0.00000000e+00, -9.45442009e-01, 5.96856167e-02,
7.19317248e-02, 6.81888149e-01, -2.77555756e-17,
1.04083409e-17
],
[
0.00000000e+00, -2.89432165e-01, -1.18596498e-01,
-6.35513913e-01, 5.24032975e-03, -6.51338823e-01,
0.00000000e+00
]]
])
panda.q = q1
nt.assert_array_almost_equal(panda.hessian0(), ans)
nt.assert_array_almost_equal(panda.hessian0(q2), ans)
nt.assert_array_almost_equal(panda.hessian0(q3), ans)
nt.assert_array_almost_equal(panda.hessian0(q4), ans)
nt.assert_array_almost_equal(panda.hessian0(J0=panda.jacob0(q1)), ans)
nt.assert_array_almost_equal(panda.hessian0(q1, J0=panda.jacob0(q1)),
ans)
# self.assertRaises(ValueError, panda.hessian0)
self.assertRaises(ValueError, panda.hessian0, [1, 3])
self.assertRaises(TypeError, panda.hessian0, 'Wfgsrth')
self.assertRaises(ValueError, panda.hessian0, [1, 3], np.array([1, 5]))
self.assertRaises(TypeError, panda.hessian0, [1, 3], 'qwe')
def test_control_type(self):
panda = rp.Panda()
panda.control_type = 'v'
self.assertEqual(panda.control_type, 'v')