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')