rospy.init_node("test_robot")
r = ArmInterface(
) # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object)
cm = r.get_controller_manager(
) # get controller manager instance associated with the robot (not required in most cases)
mvt = r.get_movegroup_interface(
) # get the moveit interface for planning and executing trajectories using moveit planners (see https://justagist.github.io/franka_ros_interface/DOC.html#franka_moveit.PandaMoveGroupInterface for documentation)
elapsed_time_ = rospy.Duration(0.0)
period = rospy.Duration(0.005)
r.move_to_neutral() # move robot to neutral pose
initial_pose = r.joint_angles() # get current joint angles of the robot
jac = r.zero_jacobian() # get end-effector jacobian
count = 0
rate = rospy.Rate(1000)
rospy.loginfo("Commanding...\n")
joint_names = r.joint_names()
vals = r.joint_angles()
while not rospy.is_shutdown():
elapsed_time_ += period
delta = 3.14 / 16.0 * (
1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2
for j in joint_names:
time_cycle_start = rospy.get_time() # in seconds #
time_cycle_start_wall = time.time() # in seconds #
# Get joint values from robot
vals_pos = robot.joint_angles()
for i, j in enumerate(joint_names):
q[i] = vals_pos[j]
# End of the SENSING
t_robot_sense = time.time() - time_cycle_start_wall
# Start of MATRIX COMPUTATION
t_matrix_start = time.time()
# Compute the jacobian of the robot
o_J_n = robot.zero_jacobian()
# Note that, to be fully formally correct, I would need the Jacobian in the
# end-effector frame, see e.g. https://hal.inria.fr/inria-00351899/document
# However ArmInterface gives me the Jacobian in the robot base frame,
# thus I need to properly handle this with a proper transformation matrix
_, n_q_o = tf_listener.lookupTransform('panda_hand', 'base',rospy.Time())
n_R_o = tf.transformations.quaternion_matrix(n_q_o)[0:3, 0:3]
n_V_o = np.zeros((6, 6))
n_V_o[0:3, 0:3] = n_R_o
n_V_o[3:6, 3:6] = n_R_o
Jn = np.matmul(n_V_o, o_J_n)
VJn = np.matmul(V, Jn)