diff --git a/.gitignore b/.gitignore index 4c96b68..12829e2 100644 --- a/.gitignore +++ b/.gitignore @@ -126,7 +126,6 @@ dmypy.json .pyre/ # ROS -catkin_ws/devel -catkin_ws/build -catkin_ws/logs -catkin_ws/.catkin_tools \ No newline at end of file +ros2_ws/build +ros2_ws/install +ros2_ws/log \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index 0cfc6c7..95b96a9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "catkin_ws/src/franka-interface-msgs"] path = catkin_ws/src/franka-interface-msgs url = https://github.com/iamlab-cmu/franka-interface-msgs.git +[submodule "ros2_ws/src/franka_interface_msgs"] + path = ros2_ws/src/franka_interface_msgs + url = https://github.com/iamlab-cmu/franka_interface_msgs.git diff --git a/README.md b/README.md index 87c5041..85f68b5 100644 --- a/README.md +++ b/README.md @@ -27,9 +27,9 @@ Do `nproc` to find out how many cores you have, and use that as the `N` number i ```shell sudo apt-get install autoconf automake libtool curl make g++ unzip -wget https://github.com/protocolbuffers/protobuf/releases/download/v3.11.4/protobuf-all-3.11.4.zip -unzip protobuf-all-3.11.4.zip -cd protobuf-3.11.4 +wget https://github.com/protocolbuffers/protobuf/releases/download/v21.8/protobuf-all-21.8.zip +unzip protobuf-all-21.8.zip +cd protobuf-21.8 ./configure make -jN make check -jN diff --git a/bash_scripts/make_catkin.sh b/bash_scripts/make_catkin.sh deleted file mode 100755 index 3b69c16..0000000 --- a/bash_scripts/make_catkin.sh +++ /dev/null @@ -1,3 +0,0 @@ -cd catkin_ws -catkin build -cd .. \ No newline at end of file diff --git a/bash_scripts/make_ros2_ws.sh b/bash_scripts/make_ros2_ws.sh new file mode 100755 index 0000000..2590fb8 --- /dev/null +++ b/bash_scripts/make_ros2_ws.sh @@ -0,0 +1,3 @@ +cd ros2_ws +colcon build +cd .. \ No newline at end of file diff --git a/bash_scripts/start_control_pc.sh b/bash_scripts/start_control_pc.sh index 4007812..ecdb8b2 100755 --- a/bash_scripts/start_control_pc.sh +++ b/bash_scripts/start_control_pc.sh @@ -90,18 +90,6 @@ fi DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" -# Start rosmaster in a new gnome-terminal if not already running -if ! pgrep -x "roscore" > /dev/null -then - start_rosmaster_path="$DIR/start_rosmaster.sh" - echo "Will start ROS master in new terminal."$start_rosmaster_path - gnome-terminal --working-directory="$DIR" -- bash $start_rosmaster_path - sleep 3 - echo "Did start ROS master in new terminal." -else - echo "Roscore is already running" -fi - if [ "$with_gripper" -eq 0 ]; then let old_gripper=0 fi diff --git a/bash_scripts/start_franka_gripper_on_control_pc.sh b/bash_scripts/start_franka_gripper_on_control_pc.sh index a25c8f3..400cfaf 100755 --- a/bash_scripts/start_franka_gripper_on_control_pc.sh +++ b/bash_scripts/start_franka_gripper_on_control_pc.sh @@ -9,30 +9,28 @@ robot_ip=${6} control_pc_use_password=${7} control_pc_password=${8} -rosmaster_path="bash_scripts/set_rosmaster.sh" -catkin_ws_setup_path="catkin_ws/devel/setup.bash" - if [ "$control_pc_ip_address" = "localhost" ]; then cd $HOME cd $control_pc_franka_interface_path - source $catkin_ws_setup_path - roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip + cd ros2_ws + source install/setup.bash + ros2 launch franka_ros_interface franka_gripper.launch.py robot_num:=$robot_number robot_ip:=$robot_ip bash else if [ "$control_pc_use_password" = "0" ]; then ssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH cd $control_pc_franka_interface_path -source $rosmaster_path $control_pc_ip_address $workstation_ip_address -source $catkin_ws_setup_path -roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip +cd ros2_ws +source install/setup.bash +ros2 launch franka_ros_interface franka_gripper.launch.py robot_num:=$robot_number robot_ip:=$robot_ip bash EOSSH else sshpass -p "$control_pc_password" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH cd $control_pc_franka_interface_path -source $rosmaster_path $control_pc_ip_address $workstation_ip_address -source $catkin_ws_setup_path -roslaunch franka_ros_interface franka_gripper.launch robot_num:=$robot_number robot_ip:=$robot_ip +cd ros2_ws +source install/setup.bash +ros2 launch franka_ros_interface franka_gripper.launch.py robot_num:=$robot_number robot_ip:=$robot_ip bash EOSSH fi diff --git a/bash_scripts/start_franka_ros_interface_on_control_pc.sh b/bash_scripts/start_franka_ros_interface_on_control_pc.sh index 5e9b692..d6eb06d 100755 --- a/bash_scripts/start_franka_ros_interface_on_control_pc.sh +++ b/bash_scripts/start_franka_ros_interface_on_control_pc.sh @@ -8,30 +8,31 @@ robot_number=${5} control_pc_use_password=${6} control_pc_password=${7} -rosmaster_path="bash_scripts/set_rosmaster.sh" -catkin_ws_setup_path="catkin_ws/devel/setup.bash" - if [ "$control_pc_ip_address" = "localhost" ]; then cd $HOME cd $control_pc_franka_interface_path - source $catkin_ws_setup_path - roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number + cd ros2_ws + export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/home/$control_pc_uname/$control_pc_franka_interface_path/build/franka-interface-common:/home/$control_pc_uname/$control_pc_franka_interface_path/build/franka-interface/proto + source install/setup.bash + ros2 launch franka_ros_interface franka_ros_interface.launch.py robot_num:=$robot_number bash else if [ "$control_pc_use_password" = "0" ]; then ssh -tt $control_pc_uname@$control_pc_ip_address << EOSSH cd $control_pc_franka_interface_path -source $rosmaster_path $control_pc_ip_address $workstation_ip_address -source $catkin_ws_setup_path -roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number +cd ros2_ws +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/home/$control_pc_uname/$control_pc_franka_interface_path/build/franka-interface-common:/home/$control_pc_uname/$control_pc_franka_interface_path/build/franka-interface/proto +source install/setup.bash +ros2 launch franka_ros_interface franka_ros_interface.launch.py robot_num:=$robot_number bash EOSSH else sshpass -p "$control_pc_password" ssh -tt -o StrictHostKeyChecking=no $control_pc_uname@$control_pc_ip_address << EOSSH cd $control_pc_franka_interface_path -source $rosmaster_path $control_pc_ip_address $workstation_ip_address -source $catkin_ws_setup_path -roslaunch franka_ros_interface franka_ros_interface.launch robot_num:=$robot_number +cd ros2_ws +export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/home/$control_pc_uname/$control_pc_franka_interface_path/build/franka-interface-common:/home/$control_pc_uname/$control_pc_franka_interface_path/build/franka-interface/proto +source install/setup.bash +ros2 launch franka_ros_interface franka_ros_interface.launch.py robot_num:=$robot_number bash EOSSH fi diff --git a/bash_scripts/start_rosmaster.sh b/bash_scripts/start_rosmaster.sh deleted file mode 100755 index 99d42a0..0000000 --- a/bash_scripts/start_rosmaster.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -roscore \ No newline at end of file diff --git a/catkin_ws/src/franka-interface-msgs b/catkin_ws/src/franka-interface-msgs deleted file mode 160000 index 6135983..0000000 --- a/catkin_ws/src/franka-interface-msgs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 61359833c216b0d63f578fbc425b621159a054d4 diff --git a/cfg/april_tag_pick_place_cfg.yaml b/cfg/april_tag_pick_place_cfg.yaml deleted file mode 100644 index 35fdf80..0000000 --- a/cfg/april_tag_pick_place_cfg.yaml +++ /dev/null @@ -1,25 +0,0 @@ -rs: - id: 0 - frame: realsense - filter_depth: True - -april_tag: - detector: - families: tag36h11 - border: 1 - nthreads: 4 - quad_decimate: 1.0 - quad_blur: 0.0 - refine_edges: True - refine_decode: False - refine_pose: False - debug: False - quad_contours: True - tag_size: 0.0254 # 1 inch in meters - -T_camera_ee_path: examples/T_RSD415_franka.tf -T_camera_mount_path: examples/T_RS_mount_delta.tf - -cube_size: 0.04 - -vis_detect: True diff --git a/docs/conf.py b/docs/conf.py index 632d348..1075ea2 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -40,7 +40,7 @@ autoclass_content = "class" autodoc_member_order = "bysource" autodoc_default_flags = ["members", "show-inheritance"] -#autodoc_mock_imports = ["numpy", "scipy", "matplotlib", "matplotlib.pyplot", "scipy.interpolate", "autolab_core", "quaternion", "itertools", "roslib", "rospy", "actionlib", "sensor_msgs", "franka_interface_msgs", "franka_gripper"] +#autodoc_mock_imports = ["numpy", "scipy", "matplotlib", "matplotlib.pyplot", "scipy.interpolate", "autolab_core", "quaternion", "itertools", "rclpy", "actionlib", "sensor_msgs", "franka_interface_msgs", "franka_gripper"] napoleon_include_special_with_doc = True napoleon_include_init_with_doc = True diff --git a/docs/install.rst b/docs/install.rst index 97b9b4b..b352b3a 100644 --- a/docs/install.rst +++ b/docs/install.rst @@ -52,9 +52,9 @@ If you plan on modifying the library and the protobuf messages, you will need to 2. Execute the following commands:: sudo apt-get install autoconf automake libtool curl make g++ unzip - wget https://github.com/protocolbuffers/protobuf/releases/download/v3.11.4/protobuf-all-3.11.4.zip - unzip protobuf-all-3.11.4.zip - cd protobuf-3.11.4 + wget https://github.com/protocolbuffers/protobuf/releases/download/v21.8/protobuf-all-21.8.zip + unzip protobuf-all-21.8.zip + cd protobuf-21.8 ./configure 3. Use the number that was previously printed out using the ``nproc`` command above and substitute it as ``N`` below:: diff --git a/examples/example_movements.py b/examples/example_movements.py deleted file mode 100644 index 07ae620..0000000 --- a/examples/example_movements.py +++ /dev/null @@ -1,45 +0,0 @@ -from frankapy import FrankaArm -import numpy as np -from autolab_core import RigidTransform - -if __name__ == '__main__': - - print('Starting robot') - fa = FrankaArm() - - xtranslation_3cm = RigidTransform(rotation=np.array([ - [1, 0, 0], - [0, 1, 0], - [0, 0, 1] - ]), translation=np.array([0.03, 0, 0]), - from_frame='franka_tool', to_frame='world') - - random_position = RigidTransform(rotation=np.array([ - [0.9323473, -0.35858258, 0.04612846], - [-0.35996283, -0.93259467, 0.02597504], - [0.03370496, -0.04082229, -0.99859775] - ]), translation=np.array([0.39247965, -0.21613652, 0.3882055]), - from_frame='franka_tool', to_frame='world') - - print(fa.get_pose().translation) - - print(fa.get_joints()) - - desired_joints_1 = [-0.52733715, 0.25603565, 0.47721503, -1.26705864, 0.00600359, 1.60788199, 0.63019184] - - desired_joints_2 = [-0.16017485, 1.12476619, 0.26004398, -0.67246923, 0.04899213, 2.08439578, 0.81627789] - - fa.reset_joints() - - print('Opening Grippers') - fa.open_gripper() - - #fa.reset_pose() - - # fa.goto_pose(random_position, use_impedance=False, cartesian_impedances=[3000, 3000, 100, 300, 300, 300]) - - fa.goto_joints(desired_joints_1, joint_impedances=[100, 100, 100, 50, 50, 100, 100]) - - fa.goto_joints(desired_joints_2, joint_impedances=[100, 100, 100, 50, 50, 100, 100]) - - #fa.apply_effector_forces_torques(10.0, 0, 0, 0) \ No newline at end of file diff --git a/examples/record_trajectory.py b/examples/record_trajectory.py deleted file mode 100644 index f7e0576..0000000 --- a/examples/record_trajectory.py +++ /dev/null @@ -1,25 +0,0 @@ -import argparse -import time -from frankapy import FrankaArm -import pickle as pkl - -if __name__ == '__main__': - parser = argparse.ArgumentParser() - parser.add_argument('--time', '-t', type=float, default=10) - parser.add_argument('--open_gripper', '-o', action='store_true') - parser.add_argument('--file', '-f', default='franka_traj.pkl') - args = parser.parse_args() - - print('Starting robot') - fa = FrankaArm() - if args.open_gripper: - fa.open_gripper() - print('Applying 0 force torque control for {}s'.format(args.time)) - end_effector_position = [] - fa.run_guide_mode(args.time, block=False) - - for i in range(1000): - end_effector_position.append(fa.get_pose()) - time.sleep(0.01) - - pkl.dump(end_effector_position, open(args.file, 'wb')) \ No newline at end of file diff --git a/examples/run_dynamic_joint_velocities.py b/examples/run_dynamic_joint_velocities.py new file mode 100644 index 0000000..8a5ab94 --- /dev/null +++ b/examples/run_dynamic_joint_velocities.py @@ -0,0 +1,64 @@ +import numpy as np +import time + +from frankapy import FrankaArm, SensorDataMessageType +from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg +from frankapy.proto import JointVelocitySensorMessage, ShouldTerminateSensorMessage +from franka_interface_msgs.msg import SensorDataGroup + +from frankapy.utils import min_jerk + +if __name__ == "__main__": + fa = FrankaArm() + fa.reset_joints() + + fa.log_info('Generating Trajectory') + p = fa.get_pose() + p.translation[2] -= 0.2 + fa.goto_pose(p, duration=5, block=False) + + joint_accelerations = [1, 1, 1, 1, 1, 1, 1] + + T = 5 + dt = 0.01 + ts = np.arange(0, T, dt) + joint_velocities = [] + + for i in range(len(ts)): + joint_velocities.append(fa.get_robot_state()['joint_velocities']) + time.sleep(dt) + + fa.wait_for_skill() + + fa.reset_joints() + + fa.log_info('Initializing Sensor Publisher') + + fa.log_info('Publishing joints trajectory...') + # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed + fa.execute_joint_velocities(joint_velocities[0], joint_accelerations, duration=T, dynamic=True, buffer_time=10) + init_time = fa.get_time() + for i in range(len(ts)): + traj_gen_proto_msg = JointVelocitySensorMessage( + id=i, timestamp=fa.get_time() - init_time, + joint_velocities=joint_velocities[i] + ) + ros_msg = make_sensor_group_msg( + trajectory_generator_sensor_msg=sensor_proto2ros_msg( + traj_gen_proto_msg, SensorDataMessageType.JOINT_VELOCITY) + ) + + fa.log_info('Publishing: ID {}'.format(traj_gen_proto_msg.id)) + fa.publish_sensor_data(ros_msg) + time.sleep(dt) + + # Stop the skill + # Alternatively can call fa.stop_skill() + term_proto_msg = ShouldTerminateSensorMessage(timestamp=fa.get_time() - init_time, should_terminate=True) + ros_msg = make_sensor_group_msg( + termination_handler_sensor_msg=sensor_proto2ros_msg( + term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) + ) + fa.publish_sensor_data(ros_msg) + + fa.log_info('Done') diff --git a/examples/run_dynamic_joints.py b/examples/run_dynamic_joints.py index 3eb37b4..cd280d5 100644 --- a/examples/run_dynamic_joints.py +++ b/examples/run_dynamic_joints.py @@ -1,21 +1,18 @@ import numpy as np +import time from frankapy import FrankaArm, SensorDataMessageType -from frankapy import FrankaConstants as FC from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg from frankapy.proto import JointPositionSensorMessage, ShouldTerminateSensorMessage from franka_interface_msgs.msg import SensorDataGroup from frankapy.utils import min_jerk -import rospy - - if __name__ == "__main__": fa = FrankaArm() fa.reset_joints() - rospy.loginfo('Generating Trajectory') + fa.log_info('Generating Trajectory') joints_0 = fa.get_joints() p = fa.get_pose() p.translation[2] -= 0.2 @@ -27,17 +24,15 @@ ts = np.arange(0, T, dt) joints_traj = [min_jerk(joints_1, joints_0, t, T) for t in ts] - rospy.loginfo('Initializing Sensor Publisher') - pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000) - rate = rospy.Rate(1 / dt) + fa.log_info('Initializing Sensor Publisher') - rospy.loginfo('Publishing joints trajectory...') + fa.log_info('Publishing joints trajectory...') # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed fa.goto_joints(joints_traj[1], duration=T, dynamic=True, buffer_time=10) - init_time = rospy.Time.now().to_time() + init_time = fa.get_time() for i in range(2, len(ts)): traj_gen_proto_msg = JointPositionSensorMessage( - id=i, timestamp=rospy.Time.now().to_time() - init_time, + id=i, timestamp=fa.get_time() - init_time, joints=joints_traj[i] ) ros_msg = make_sensor_group_msg( @@ -45,17 +40,17 @@ traj_gen_proto_msg, SensorDataMessageType.JOINT_POSITION) ) - rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id)) - pub.publish(ros_msg) - rate.sleep() + fa.log_info('Publishing: ID {}'.format(traj_gen_proto_msg.id)) + fa.publish_sensor_data(ros_msg) + time.sleep(dt) # Stop the skill # Alternatively can call fa.stop_skill() - term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True) + term_proto_msg = ShouldTerminateSensorMessage(timestamp=fa.get_time() - init_time, should_terminate=True) ros_msg = make_sensor_group_msg( termination_handler_sensor_msg=sensor_proto2ros_msg( term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) ) - pub.publish(ros_msg) + fa.publish_sensor_data(ros_msg) - rospy.loginfo('Done') + fa.log_info('Done') diff --git a/examples/run_dynamic_pose.py b/examples/run_dynamic_pose.py index a2e0a07..019be5a 100644 --- a/examples/run_dynamic_pose.py +++ b/examples/run_dynamic_pose.py @@ -1,4 +1,5 @@ import numpy as np +import time from autolab_core import RigidTransform from frankapy import FrankaArm, SensorDataMessageType @@ -9,14 +10,11 @@ from frankapy.utils import min_jerk, min_jerk_weight -import rospy - - if __name__ == "__main__": fa = FrankaArm() fa.reset_joints() - rospy.loginfo('Generating Trajectory') + fa.log_info('Generating Trajectory') p0 = fa.get_pose() p1 = p0.copy() T_delta = RigidTransform( @@ -37,18 +35,17 @@ z_stiffness_traj = [min_jerk(100, 800, t, T) for t in ts] - rospy.loginfo('Initializing Sensor Publisher') - pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1000) - rate = rospy.Rate(1 / dt) + fa.log_info('Initializing Sensor Publisher') - rospy.loginfo('Publishing pose trajectory...') + fa.log_info('Publishing pose trajectory...') # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed fa.goto_pose(pose_traj[1], duration=T, dynamic=True, buffer_time=10, cartesian_impedances=FC.DEFAULT_TRANSLATIONAL_STIFFNESSES[:2] + [z_stiffness_traj[1]] + FC.DEFAULT_ROTATIONAL_STIFFNESSES ) - init_time = rospy.Time.now().to_time() + init_time = fa.get_time() + for i in range(2, len(ts)): - timestamp = rospy.Time.now().to_time() - init_time + timestamp = fa.get_time() - init_time traj_gen_proto_msg = PosePositionSensorMessage( id=i, timestamp=timestamp, position=pose_traj[i].translation, quaternion=pose_traj[i].quaternion @@ -73,20 +70,19 @@ if current_gripper_width < 0.002: has_closed = True - fa.goto_gripper(current_gripper_width, block=False) - + #fa.goto_gripper(current_gripper_width, block=False) - rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id)) - pub.publish(ros_msg) - rate.sleep() + fa.log_info('Publishing: ID {}'.format(traj_gen_proto_msg.id)) + fa.publish_sensor_data(ros_msg) + time.sleep(dt) # Stop the skill # Alternatively can call fa.stop_skill() - term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True) + term_proto_msg = ShouldTerminateSensorMessage(timestamp=fa.get_time() - init_time, should_terminate=True) ros_msg = make_sensor_group_msg( termination_handler_sensor_msg=sensor_proto2ros_msg( term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) ) - pub.publish(ros_msg) + fa.publish_sensor_data(ros_msg) - rospy.loginfo('Done') + fa.log_info('Done') diff --git a/examples/run_hfpc.py b/examples/run_hfpc.py index 2177c6d..2f1c56d 100644 --- a/examples/run_hfpc.py +++ b/examples/run_hfpc.py @@ -1,4 +1,5 @@ import numpy as np +import time from frankapy import FrankaArm, SensorDataMessageType from frankapy import FrankaConstants as FC @@ -9,16 +10,13 @@ from tqdm import trange -import rospy - - if __name__ == "__main__": fa = FrankaArm() fa.reset_joints() fa.close_gripper() while True: - input('Presse [Enter] to enter guide mode and move robot to be on top of a flat surface.') + input('Press [Enter] to enter guide mode and move robot to be on top of a flat surface.') fa.run_guide_mode() while True: inp = input('[r]etry or [c]ontinue? ') @@ -29,7 +27,7 @@ if inp == 'c': break - rospy.loginfo('Generating Trajectory') + fa.log_info('Generating Trajectory') # EE will follow a 2D circle while pressing down with a target force dt = 0.01 T = 10 @@ -48,26 +46,23 @@ pose.translation[1] += circ[i, 1] target_poses.append(pose) target_force = [0, 0, -10, 0, 0, 0] - S = [1, 1, 1, 1, 1, 1] + S = [1, 1, 0, 1, 1, 1] position_kps_cart = FC.DEFAULT_TRANSLATIONAL_STIFFNESSES + FC.DEFAULT_ROTATIONAL_STIFFNESSES force_kps_cart = [0.1] * 6 position_kps_joint = FC.DEFAULT_K_GAINS force_kps_joint = [0.1] * 7 - rospy.loginfo('Initializing Sensor Publisher') - pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=1) - rate = rospy.Rate(1 / dt) n_times = 1 - rospy.loginfo('Publishing HFPC trajectory w/ cartesian gains...') + fa.log_info('Publishing HFPC trajectory w/ cartesian gains...') fa.run_dynamic_force_position(duration=T * n_times, buffer_time=3, S=S, use_cartesian_gains=True, position_kps_cart=position_kps_cart, force_kps_cart=force_kps_cart) - init_time = rospy.Time.now().to_time() + init_time = fa.get_time() for i in trange(N * n_times): t = i % N - timestamp = rospy.Time.now().to_time() - init_time + timestamp = fa.get_time() - init_time traj_gen_proto_msg = ForcePositionSensorMessage( id=i, timestamp=timestamp, seg_run_time=dt, pose=transform_to_list(target_poses[t]), @@ -85,19 +80,19 @@ feedback_controller_sensor_msg=sensor_proto2ros_msg( fb_ctrlr_proto, SensorDataMessageType.FORCE_POSITION_GAINS) ) - pub.publish(ros_msg) - rate.sleep() + fa.publish_sensor_data(ros_msg) + time.sleep(dt) fa.stop_skill() - rospy.loginfo('Publishing HFPC trajectory w/ joint gains...') + fa.log_info('Publishing HFPC trajectory w/ joint gains...') fa.run_dynamic_force_position(duration=T * n_times, buffer_time=3, S=S, use_cartesian_gains=False, position_kps_joint=position_kps_joint, force_kps_joint=force_kps_joint) - init_time = rospy.Time.now().to_time() + init_time = fa.get_time() for i in trange(N * n_times): t = i % N - timestamp = rospy.Time.now().to_time() - init_time + timestamp = fa.get_time() - init_time traj_gen_proto_msg = ForcePositionSensorMessage( id=i, timestamp=timestamp, seg_run_time=dt, pose=transform_to_list(target_poses[t]), @@ -115,11 +110,11 @@ feedback_controller_sensor_msg=sensor_proto2ros_msg( fb_ctrlr_proto, SensorDataMessageType.FORCE_POSITION_GAINS) ) - pub.publish(ros_msg) - rate.sleep() + fa.publish_sensor_data(ros_msg) + time.sleep(dt) fa.stop_skill() fa.reset_joints() fa.open_gripper() - rospy.loginfo('Done') + fa.log_info('Done') diff --git a/examples/run_pose_dmp.py b/examples/run_pose_dmp.py index 3327c4f..3b75731 100644 --- a/examples/run_pose_dmp.py +++ b/examples/run_pose_dmp.py @@ -1,9 +1,5 @@ -import numpy as np -import math -import rospy import argparse import pickle -from autolab_core import RigidTransform, Point from frankapy import FrankaArm if __name__ == '__main__': diff --git a/examples/run_quaternion_pose_dmp.py b/examples/run_quaternion_pose_dmp.py index 8bd5c48..8cd7b23 100644 --- a/examples/run_quaternion_pose_dmp.py +++ b/examples/run_quaternion_pose_dmp.py @@ -1,9 +1,5 @@ -import numpy as np -import math -import rospy import argparse import pickle -from autolab_core import RigidTransform, Point from frankapy import FrankaArm def execute_quaternion_pose_dmp(fa, position_dmp_weights_path, quat_dmp_weights_path, diff --git a/examples/run_recorded_trajectory.py b/examples/run_recorded_trajectory.py deleted file mode 100644 index df16f93..0000000 --- a/examples/run_recorded_trajectory.py +++ /dev/null @@ -1,61 +0,0 @@ -import pickle as pkl -import numpy as np - -from autolab_core import RigidTransform -from frankapy import FrankaArm, SensorDataMessageType -from frankapy import FrankaConstants as FC -from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg -from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage -from franka_interface_msgs.msg import SensorDataGroup - -from frankapy.utils import min_jerk, min_jerk_weight - -import rospy - -if __name__ == "__main__": - fa = FrankaArm() - fa.reset_joints() - - rospy.loginfo('Generating Trajectory') - - pose_traj = pkl.load(open('franka_traj.pkl','rb')) - - T = 10 - dt = 0.01 - ts = np.arange(0, T, dt) - - rospy.loginfo('Initializing Sensor Publisher') - pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=10) - rate = rospy.Rate(1 / dt) - - rospy.loginfo('Publishing pose trajectory...') - # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed - fa.goto_pose(pose_traj[1], duration=T, dynamic=True, buffer_time=10, - cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0] - ) - init_time = rospy.Time.now().to_time() - for i in range(2, len(ts)): - timestamp = rospy.Time.now().to_time() - init_time - traj_gen_proto_msg = PosePositionSensorMessage( - id=i, timestamp=timestamp, - position=pose_traj[i].translation, quaternion=pose_traj[i].quaternion - ) - ros_msg = make_sensor_group_msg( - trajectory_generator_sensor_msg=sensor_proto2ros_msg( - traj_gen_proto_msg, SensorDataMessageType.POSE_POSITION), - ) - - rospy.loginfo('Publishing: ID {}'.format(traj_gen_proto_msg.id)) - pub.publish(ros_msg) - rate.sleep() - - # Stop the skill - # Alternatively can call fa.stop_skill() - term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, should_terminate=True) - ros_msg = make_sensor_group_msg( - termination_handler_sensor_msg=sensor_proto2ros_msg( - term_proto_msg, SensorDataMessageType.SHOULD_TERMINATE) - ) - pub.publish(ros_msg) - - rospy.loginfo('Done') diff --git a/frankapy/__init__.py b/frankapy/__init__.py index 610df33..fd57183 100644 --- a/frankapy/__init__.py +++ b/frankapy/__init__.py @@ -1,5 +1,7 @@ from .franka_arm import FrankaArm from .franka_constants import FrankaConstants -from .franka_arm_state_client import FrankaArmStateClient +from .gripper_state_client import GripperStateClient +from .franka_robot_state_client import FrankaRobotStateClient +from .franka_interface_status_client import FrankaInterfaceStatusClient from .exceptions import FrankaArmCommException from .franka_interface_common_definitions import SkillType, MetaSkillType, TrajectoryGeneratorType, FeedbackControllerType, TerminationHandlerType, SkillStatus, SensorDataMessageType \ No newline at end of file diff --git a/frankapy/franka_arm.py b/frankapy/franka_arm.py index ff3c562..c67fc33 100644 --- a/frankapy/franka_arm.py +++ b/frankapy/franka_arm.py @@ -11,46 +11,44 @@ import quaternion from itertools import product -import roslib -roslib.load_manifest('franka_interface_msgs') -import rospy -from actionlib import SimpleActionClient +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + from sensor_msgs.msg import JointState -from franka_interface_msgs.msg import ExecuteSkillAction -from franka_interface_msgs.srv import GetCurrentFrankaInterfaceStatusCmd -from franka_gripper.msg import * +from franka_interface_msgs.msg import SensorDataGroup +from franka_interface_msgs.action import ExecuteSkill +from franka_msgs.action import Homing, Move, Grasp from .skill_list import * from .exceptions import * -from .franka_arm_state_client import FrankaArmStateClient +from .gripper_state_client import GripperStateClient +from .franka_robot_state_client import FrankaRobotStateClient +from .franka_interface_status_client import FrankaInterfaceStatusClient from .franka_constants import FrankaConstants as FC from .franka_interface_common_definitions import * -from .ros_utils import BoxesPublisher +from .ros_utils import CollisionBoxesPublisher -class FrankaArm: +class FrankaArm(Node): def __init__( self, - rosnode_name='franka_arm_client', - ros_log_level=rospy.INFO, + node_name='franka_arm_client', robot_num=1, with_gripper=True, old_gripper=False, offline=False, - init_node=True): + init_rclpy=True): """ Initialize a FrankaArm. Parameters ---------- - rosnode_name : :obj:`str` + node_name : :obj:`str` A name for the FrankaArm ROS Node. - ros_log_level : :obj:`rospy.log_level` of int - Passed to rospy.init_node. - robot_num : :obj:`int` Number assigned to the current robot. @@ -66,69 +64,73 @@ def __init__( """ + if init_rclpy: + rclpy.init() + super().__init__(node_name) + + # 30 means WARN while 20 means INFO + self.get_logger().set_level(20) + self._execute_skill_action_server_name = \ - '/execute_skill_action_server_node_{}/execute_skill'.format(robot_num) + 'execute_skill_action_server_node_{}/execute_skill'.format(robot_num) + self._gripper_state_server_name = \ + '/get_current_gripper_state_server_node_{}/gripper_state'.format(robot_num) self._robot_state_server_name = \ - '/get_current_robot_state_server_node_{}/get_current_robot_state_server'.format(robot_num) + '/get_current_robot_state_server_node_{}/robot_state'.format(robot_num) self._franka_interface_status_server_name = \ - '/get_current_franka_interface_status_server_node_{}/get_current_franka_interface_status_server'.format(robot_num) + '/get_current_franka_interface_status_server_node_{}/franka_interface_status'.format(robot_num) self._gripper_homing_action_server_name = \ '/franka_gripper_{}/homing'.format(robot_num) self._gripper_move_action_server_name = \ '/franka_gripper_{}/move'.format(robot_num) - self._gripper_stop_action_server_name = \ - '/franka_gripper_{}/stop'.format(robot_num) self._gripper_grasp_action_server_name = \ '/franka_gripper_{}/grasp'.format(robot_num) self._gripper_joint_states_name = \ '/franka_gripper_{}/joint_states'.format(robot_num) + self._joint_state_publisher_name = \ + '/franka_virtual_joints_{}'.format(robot_num) + self._sensor_data_publisher_name = \ + '/sensor_data_{}/sensor_data'.format(robot_num) self._connected = False self._in_skill = False + self._in_gripper_skill = False self._offline = offline self._with_gripper = with_gripper self._old_gripper = old_gripper - self._last_gripper_command = None - - # init ROS - if init_node: - rospy.init_node(rosnode_name, - disable_signals=True, - log_level=ros_log_level) - self._collision_boxes_pub = BoxesPublisher('franka_collision_boxes_{}'.format(robot_num)) - self._joint_state_pub = rospy.Publisher('franka_virtual_joints_{}'.format(robot_num), JointState, queue_size=10) + + self._collision_boxes_pub = CollisionBoxesPublisher('franka_collision_boxes_{}'.format(robot_num)) + self._sensor_data_pub = self.create_publisher(SensorDataGroup, self._sensor_data_publisher_name, 10) + self._joint_state_pub = self.create_publisher(JointState, self._joint_state_publisher_name, 10) - self._state_client = FrankaArmStateClient( - new_ros_node=False, + self._robot_state_client = FrankaRobotStateClient( robot_state_server_name=self._robot_state_server_name, offline=self._offline) + self._franka_interface_status_client = FrankaInterfaceStatusClient( + franka_interface_status_server_name=self._franka_interface_status_server_name, + offline=self._offline) + if not self._offline: # set signal handler to handle ctrl+c and kill sigs signal.signal(signal.SIGINT, self._sigint_handler_gen()) - - rospy.wait_for_service(self._franka_interface_status_server_name) - self._get_current_franka_interface_status = rospy.ServiceProxy( - self._franka_interface_status_server_name, GetCurrentFrankaInterfaceStatusCmd) - self._client = SimpleActionClient( - self._execute_skill_action_server_name, ExecuteSkillAction) - self._client.wait_for_server() + self._execute_skill_action_client = ActionClient(self, ExecuteSkill, self._execute_skill_action_server_name) + self._execute_skill_action_client.wait_for_server() + self.wait_for_franka_interface() if self._with_gripper and not self._old_gripper: - self._gripper_homing_client = SimpleActionClient( - self._gripper_homing_action_server_name, HomingAction) + + self._gripper_homing_client = ActionClient(self, Homing, self._gripper_homing_action_server_name) self._gripper_homing_client.wait_for_server() - self._gripper_move_client = SimpleActionClient( - self._gripper_move_action_server_name, MoveAction) + self._gripper_move_client = ActionClient(self, Move, self._gripper_move_action_server_name) self._gripper_move_client.wait_for_server() - self._gripper_grasp_client = SimpleActionClient( - self._gripper_grasp_action_server_name, GraspAction) + self._gripper_grasp_client = ActionClient(self, Grasp, self._gripper_grasp_action_server_name) self._gripper_grasp_client.wait_for_server() - self._gripper_stop_client = SimpleActionClient( - self._gripper_stop_action_server_name, StopAction) - self._gripper_stop_client.wait_for_server() + + self._gripper_state_client = GripperStateClient(gripper_state_server_name=self._gripper_state_server_name, + offline=self._offline) # done init ROS self._connected = True @@ -167,11 +169,11 @@ def wait_for_franka_interface(self, timeout=None): timeout = FC.DEFAULT_FRANKA_INTERFACE_TIMEOUT if timeout is None else timeout t_start = time() while time() - t_start < timeout: - franka_interface_status = self._get_current_franka_interface_status().franka_interface_status + franka_interface_status = self._franka_interface_status_client.get_current_franka_interface_status() if franka_interface_status.is_ready: return - sleep(1e-2) - raise FrankaArmCommException('FrankaInterface status not ready for {}s'.format( + rclpy.spin_once(self) + raise FrankaArmCommException('Franka Interface Status is not ready for {}s'.format( FC.DEFAULT_FRANKA_INTERFACE_TIMEOUT)) def wait_for_skill(self): @@ -179,21 +181,14 @@ def wait_for_skill(self): Blocks execution until skill is done. """ while not self.is_skill_done(): - continue + rclpy.spin_once(self) def wait_for_gripper(self): """ Blocks execution until gripper is done. """ - if self._last_gripper_command == "Grasp": - done = self._gripper_grasp_client.wait_for_result() - elif self._last_gripper_command == "Homing": - done = self._gripper_homing_client.wait_for_result() - elif self._last_gripper_command == "Stop": - done = self._gripper_stop_client.wait_for_result() - elif self._last_gripper_command == "Move": - done = self._gripper_move_client.wait_for_result() - sleep(2) + while not self.is_gripper_skill_done(): + rclpy.spin_once(self) def is_skill_done(self, ignore_errors=True): @@ -210,14 +205,14 @@ def is_skill_done(self, ignore_errors=True): :obj:`bool` Flag of whether the skill is done. """ - if not self._in_skill: - return True + + rclpy.spin_once(self) - franka_interface_status = self._get_current_franka_interface_status().franka_interface_status + franka_interface_status = self._franka_interface_status_client.get_current_franka_interface_status() e = None - if rospy.is_shutdown(): - e = RuntimeError('rospy is down!') + if not rclpy.ok(): + e = RuntimeError('ROS is down!') elif franka_interface_status.error_description: e = FrankaArmException(franka_interface_status.error_description) elif not franka_interface_status.is_ready: @@ -229,26 +224,36 @@ def is_skill_done(self, ignore_errors=True): else: raise e - done = self._client.wait_for_result(rospy.Duration.from_sec( - FC.ACTION_WAIT_LOOP_TIME)) + if not self._in_skill: + return True - if done: - self._in_skill = False + return False + + def is_gripper_skill_done(self): + """ + Checks whether gripper skill is done. - return done + Returns + ------- + :obj:`bool` + Flag of whether the gripper skill is done. + """ + return not self._in_gripper_skill def stop_skill(self): """ Stops the current skill. """ if self._connected and self._in_skill: - self._client.cancel_goal() + self.goal_handle.cancel_goal() self.wait_for_skill() def _sigint_handler_gen(self): def sigint_handler(sig, frame): if self._connected and self._in_skill: - self._client.cancel_goal() + self.goal_handle.cancel_goal_async() + if self._in_gripper_skill: + self.gripper_goal_handle.cancel_goal_async() sys.exit(0) return sigint_handler @@ -291,13 +296,63 @@ def _send_goal(self, goal, cb, block=True, ignore_errors=True): raise ValueError('Cannot send another command when the previous skill is active!') self._in_skill = True - self._client.send_goal(goal, feedback_cb=cb) + self._send_goal_future = self._execute_skill_action_client.send_goal_async(goal, feedback_callback=self.feedback_callback) + self._send_goal_future.add_done_callback(self.goal_response_callback) + rclpy.spin_until_future_complete(self, self._send_goal_future) + + rclpy.spin_once(self) - if not block: + if not block: + rclpy.spin_once(self) return None self.wait_for_skill() - return self._client.get_result() + return None + + def goal_response_callback(self, future): + self.goal_handle = future.result() + if not self.goal_handle.accepted: + self._in_skill = False + self.get_logger().info('Goal rejected') + return + + self.get_logger().info('Robot Goal Accepted') + self._get_result_future = self.goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + self.get_logger().info('Robot Goal Completed') + + self._in_skill = False + return True + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info('Received Feedback') + + def gripper_goal_response_callback(self, future): + self.gripper_goal_handle = future.result() + if not self.gripper_goal_handle.accepted: + self.get_logger().info('Gripper Goal Rejected') + self._in_gripper_skill = False + return + + self.get_logger().info('Gripper Goal Accepted') + self._get_gripper_result_future = self.gripper_goal_handle.get_result_async() + self._get_gripper_result_future.add_done_callback(self.get_gripper_result_callback) + + def get_gripper_result_callback(self, future): + result = future.result().result + self.get_logger().info('Gripper Goal Completed') + + self._in_gripper_skill = False + return True + + def gripper_feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info('Received Gripper Feedback') + """ Controls @@ -401,8 +456,6 @@ def goto_pose(self, tool_base_pose.translation >= FC.WORKSPACE_WALLS[:, :3].max(axis=0)]): raise ValueError('Target pose is outside of workspace virtual walls!') - skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) - skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances) if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds): @@ -494,8 +547,6 @@ def goto_pose_delta(self, termination_handler_type=TerminationHandlerType.FinalPoseTerminationHandler, skill_desc=skill_desc) - skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) - skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances) if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds): @@ -654,8 +705,6 @@ def goto_joints(self, if not ignore_virtual_walls and self.is_joints_in_collision_with_boxes(joints): raise ValueError('Target joints in collision with virtual walls!') - skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) - skill.set_joint_impedances(use_impedance, cartesian_impedances, joint_impedances, k_gains, d_gains) if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds): @@ -675,6 +724,104 @@ def goto_joints(self, if dynamic: sleep(FC.DYNAMIC_SKILL_WAIT_TIME) + def execute_joint_velocities(self, + joint_velocities, + joint_accelerations, + duration=5, + dynamic=False, + buffer_time=FC.DEFAULT_TERM_BUFFER_TIME, + force_thresholds=None, + torque_thresholds=None, + cartesian_impedances=None, + joint_impedances=None, + block=True, + ignore_errors=True, + ignore_virtual_walls=False, + skill_desc='ExecuteJointVelocities'): + """ + Commands Arm to execute joint velocities + + Parameters + ---------- + joint_velocities : :obj:`list` + A list of 7 numbers that correspond to joint velocities in radians/second. + joint_accelerations : :obj:`list` + A list of 7 numbers that correspond to joint accelerations in radians/second^2. + duration : :obj:`float` + How much time this robot motion should take. + dynamic : :obj:`bool` + Flag that states whether the skill is dynamic. If True, it + will use our joint impedance controller and sensor values. + buffer_time : :obj:`float` + How much extra time the termination handler will wait + before stopping the skill after duration has passed. + force_thresholds : :obj:`list` + List of 6 floats corresponding to force limits on translation + (xyz) and rotation about (xyz) axes. Default is None. + If None then will not stop on contact. + torque_thresholds : :obj:`list` + List of 7 floats corresponding to torque limits on each joint. + Default is None. If None then will not stop on contact. + cartesian_impedances : :obj:`list` + List of 6 floats corresponding to impedances on translation + (xyz) and rotation about (xyz) axes. Default is None. If None + then will use default impedances. + joint_impedances : :obj:`list` + List of 7 floats corresponding to impedances on each joint. + This is used when use_impedance is False. Default is None. + If None then will use default impedances. + block : :obj:`bool` + Function blocks by default. If False, the function becomes + asynchronous and can be preempted. + ignore_errors : :obj:`bool` + Function ignores errors by default. If False, errors and some + exceptions can be thrown. + ignore_virtual_walls : :obj:`bool` + Function checks for collisions with virtual walls by default. + If False, the robot no longer checks, which may be dangerous. + skill_desc : :obj:`str` + Skill description to use for logging on the Control PC. + + Raises: + ValueError: If is_joints_reachable(joints) returns False + """ + + joint_velocities = np.array(joint_velocities).tolist() + joint_accelerations = np.array(joint_accelerations).tolist() + + if dynamic: + skill = Skill(SkillType.JointVelocitySkill, + TrajectoryGeneratorType.PassThroughJointVelocityTrajectoryGenerator, + feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController, + termination_handler_type=TerminationHandlerType.TimeTerminationHandler, + skill_desc=skill_desc) + block = False + else: + skill = Skill(SkillType.JointVelocitySkill, + TrajectoryGeneratorType.MinJerkJointVelocityTrajectoryGenerator, + feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController, + termination_handler_type=TerminationHandlerType.TimeTerminationHandler, + skill_desc=skill_desc) + + skill.set_joint_impedances(False, cartesian_impedances, joint_impedances, None, None) + + if not skill.check_for_contact_params(buffer_time, force_thresholds, torque_thresholds): + if dynamic: + skill.add_time_termination_params(buffer_time) + else: + skill.add_joint_threshold_params(buffer_time, FC.DEFAULT_JOINT_THRESHOLDS) + + skill.add_goal_joint_velocities(duration, joint_velocities, joint_accelerations) + goal = skill.create_goal() + + self._send_goal(goal, + cb=lambda x: skill.feedback_callback(x), + block=block, + ignore_errors=ignore_errors) + + if dynamic: + sleep(FC.DYNAMIC_SKILL_WAIT_TIME) + def execute_joint_dmp(self, joint_dmp_info, duration, @@ -756,8 +903,6 @@ def execute_joint_dmp(self, if initial_sensor_values is None: initial_sensor_values = np.ones(joint_dmp_info['num_sensors']).tolist() - - skill.add_initial_sensor_values(initial_sensor_values) # sensor values skill.add_joint_dmp_params(duration, joint_dmp_info, initial_sensor_values) @@ -878,8 +1023,6 @@ def execute_pose_dmp(self, else: initial_sensor_values = np.ones(6*pose_dmp_info['num_sensors']).tolist() - skill.add_initial_sensor_values(initial_sensor_values) # sensor values - skill.add_pose_dmp_params(orientation_only, position_only, ee_frame, duration, pose_dmp_info, initial_sensor_values) skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances) @@ -982,8 +1125,6 @@ def execute_quaternion_pose_dmp(self, if initial_sensor_values is None: initial_sensor_values = np.ones(3*position_dmp_info['num_sensors']).tolist() - skill.add_initial_sensor_values(initial_sensor_values) # sensor values - skill.add_quaternion_pose_dmp_params(ee_frame, duration, position_dmp_info, quat_dmp_info, initial_sensor_values, goal_quat, goal_quat_time) skill.set_cartesian_impedances(use_impedance, cartesian_impedances, joint_impedances) @@ -1073,8 +1214,6 @@ def apply_effector_forces_torques(self, feedback_controller_type=FeedbackControllerType.PassThroughFeedbackController, termination_handler_type=TerminationHandlerType.TimeTerminationHandler, skill_desc=skill_desc) - - skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_impulse_params(run_duration, acc_duration, max_translation, max_rotation, forces, torques) @@ -1152,8 +1291,6 @@ def apply_effector_forces_along_axis(self, feedback_controller_type=FeedbackControllerType.ForceAxisImpedenceFeedbackController, termination_handler_type=TerminationHandlerType.TimeTerminationHandler, skill_desc=skill_desc) - - skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_impulse_params(run_duration, acc_duration, max_translation, 0, forces.tolist(), [0, 0, 0]) @@ -1226,8 +1363,6 @@ def goto_gripper(self, TrajectoryGeneratorType.GripperTrajectoryGenerator, skill_desc=skill_desc) - skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) - skill.add_gripper_params(grasp, width, speed, force) goal = skill.create_goal() @@ -1241,18 +1376,28 @@ def goto_gripper(self, sleep(FC.GRIPPER_CMD_SLEEP_TIME) else: if grasp: - grasp_skill = GraspGoal() + grasp_skill = Grasp.Goal() grasp_skill.width = width grasp_skill.speed = speed grasp_skill.force = force grasp_skill.epsilon.inner = epsilon_inner grasp_skill.epsilon.outer = epsilon_outer - self._gripper_grasp_client.send_goal(grasp_skill) + + self._in_gripper_skill = True + self._send_gripper_goal_future = self._gripper_grasp_client.send_goal_async(grasp_skill, feedback_callback=self.gripper_feedback_callback) + self._send_gripper_goal_future.add_done_callback(self.gripper_goal_response_callback) + rclpy.spin_until_future_complete(self, self._send_gripper_goal_future) + else: - move_skill = MoveGoal() + move_skill = Move.Goal() move_skill.width = width move_skill.speed = speed - self._gripper_move_client.send_goal(move_skill) + + self._in_gripper_skill = True + self._send_gripper_goal_future = self._gripper_move_client.send_goal_async(move_skill, feedback_callback=self.gripper_feedback_callback) + self._send_gripper_goal_future.add_done_callback(self.gripper_goal_response_callback) + rclpy.spin_until_future_complete(self, self._send_gripper_goal_future) + if block: self.wait_for_gripper() @@ -1360,8 +1505,6 @@ def selective_guidance_mode(self, feedback_controller_type=FeedbackControllerType.SetInternalImpedanceFeedbackController, termination_handler_type=TerminationHandlerType.TimeTerminationHandler, skill_desc=skill_desc) - - skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_run_time(duration) @@ -1424,9 +1567,13 @@ def home_gripper(self, block=True, skill_desc='HomeGripper'): skill_desc : :obj:`str` Skill description to use for logging on control-pc. """ - self._last_gripper_command = 'Homing' - homing_skill = HomingGoal() - self._gripper_homing_client.send_goal(homing_skill) + homing_skill = Homing.Goal() + + self._in_gripper_skill = True + self._send_gripper_goal_future = self._gripper_homing_client.send_goal_async(homing_skill, feedback_callback=self.gripper_feedback_callback) + self._send_gripper_goal_future.add_done_callback(self.gripper_goal_response_callback) + rclpy.spin_until_future_complete(self, self._send_gripper_goal_future) + if block: self.wait_for_gripper() @@ -1442,9 +1589,9 @@ def stop_gripper(self, block=True, skill_desc='StopGripper'): skill_desc : :obj:`str` Skill description to use for logging on control-pc. """ - self._last_gripper_command = 'Stop' - stop_skill = StopGoal() - self._gripper_stop_client.send_goal(stop_skill) + + self.gripper_goal_handle.cancel_goal() + if block: self.wait_for_gripper() @@ -1536,7 +1683,6 @@ def run_dynamic_force_position(self, termination_handler_type=TerminationHandlerType.TimeTerminationHandler, skill_desc=skill_desc) - skill.add_initial_sensor_values(FC.EMPTY_SENSOR_VALUES) skill.add_force_position_params(position_kps_cart, force_kps_cart, position_kps_joint, force_kps_joint, S, use_cartesian_gains) skill.add_run_time(duration) @@ -1558,23 +1704,27 @@ def run_dynamic_force_position(self, def get_robot_state(self): """ + Returns the current robot state. + Returns ------- robot_state : :obj:`dict` Dict that contains all of the robot's current state information. """ - return self._state_client.get_data() + return self._robot_state_client.get_data() def get_pose(self, include_tool_offset=True): """ + Returns the current robot end-effector pose including the transform to the end of the tool. + Returns ------- pose : :obj:`autolab_core.RigidTransform` Current pose of the end-effector including the transform to the end of the tool. """ - tool_base_pose = self._state_client.get_pose() + tool_base_pose = self._robot_state_client.get_pose() if include_tool_offset: tool_pose = tool_base_pose * self._tool_delta_pose @@ -1591,7 +1741,7 @@ def get_joints(self): joint_positions : :obj:`numpy.ndarray` 7 floats that represent each joint's position in radians. """ - return self._state_client.get_joints() + return self._robot_state_client.get_joints() def get_joint_torques(self): """ @@ -1602,7 +1752,7 @@ def get_joint_torques(self): joint_torques : :obj:`numpy.ndarray` 7 floats that represent each joint's torque in Nm. """ - return self._state_client.get_joint_torques() + return self._robot_state_client.get_joint_torques() def get_joint_velocities(self): """ @@ -1613,7 +1763,7 @@ def get_joint_velocities(self): joint_velocities : :obj:`numpy.ndarray` 7 floats that represent each joint's velocity in rads/s. """ - return self._state_client.get_joint_velocities() + return self._robot_state_client.get_joint_velocities() def get_gripper_width(self): """ @@ -1625,9 +1775,9 @@ def get_gripper_width(self): Robot gripper width in meters. """ if self._old_gripper: - return self._state_client.get_gripper_width() + return self._robot_state_client.get_gripper_width() else: - gripper_data = rospy.wait_for_message(self._gripper_joint_states_name, JointState) + gripper_data = self._gripper_state_client.get_current_gripper_state() return gripper_data.position[0] + gripper_data.position[1] def get_gripper_is_grasped(self): @@ -1644,7 +1794,7 @@ def get_gripper_is_grasped(self): is_grasped : :obj:`bool` Returns True if gripper is grasping something. False otherwise. """ - return self._state_client.get_gripper_is_grasped() + return self._robot_state_client.get_gripper_is_grasped() def get_tool_base_pose(self): """ @@ -1668,7 +1818,7 @@ def get_ee_force_torque(self): A numpy ndarray of 6 floats that represents the current end-effector's sensed force_torque. """ - return self._state_client.get_ee_force_torque() + return self._robot_state_client.get_ee_force_torque() def get_finger_poses(self): """ @@ -1903,7 +2053,7 @@ def publish_joints(self, joints=None): joint_state = JointState() joint_state.name = FC.JOINT_NAMES - joint_state.header.stamp = rospy.Time.now() + joint_state.header.stamp = self.get_clock().now() if len(joints) == 7: joints = np.concatenate([joints, [0, 0]]) @@ -2089,6 +2239,43 @@ def is_joints_reachable(self, joints): return True + def publish_sensor_data(self, sensor_data_msg): + """ + Publishes a sensor data msg. + + Parameters + ---------- + sensor_data_msg : :obj:`SensorDataGroup msg` + Desired sensor data group msg to publish. + """ + + self._sensor_data_pub.publish(sensor_data_msg) + + def log_info(self, msg): + """ + Logs a string to ROS INFO. + + Parameters + ---------- + msg : :obj:`string` + Desired msg string to write to ROS INFO. + """ + + self.get_logger().info(msg) + + def get_time(self): + """ + Returns the current ROS Time in seconds + + Returns + ------- + time : :obj:`float` + Current ROS Time in seconds. + """ + + return self.get_clock().now().nanoseconds / 1e9 + + """ Unimplemented """ diff --git a/frankapy/franka_constants.py b/frankapy/franka_constants.py index 42fea2c..7ecf1e5 100644 --- a/frankapy/franka_constants.py +++ b/frankapy/franka_constants.py @@ -11,8 +11,6 @@ class FrankaConstants: LOGGING_LEVEL = logging.INFO - EMPTY_SENSOR_VALUES = [0] - # translational stiffness, rotational stiffness DEFAULT_FORCE_AXIS_TRANSLATIONAL_STIFFNESS = 600 DEFAULT_FORCE_AXIS_ROTATIONAL_STIFFNESS = 20 @@ -43,8 +41,8 @@ class FrankaConstants: DEFAULT_JOINT_THRESHOLDS = [0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001, 0.0001] GRIPPER_WIDTH_MAX = 0.08 - GRIPPER_WIDTH_MIN = 0 - GRIPPER_MAX_FORCE = 60 + GRIPPER_WIDTH_MIN = 0.0 + GRIPPER_MAX_FORCE = 60.0 MAX_LIN_MOMENTUM = 20 MAX_ANG_MOMENTUM = 2 @@ -177,7 +175,6 @@ class FrankaConstants: [0.2, 0, -0.05, 0, 0, 0, 1.2, 1, 0.01] ]) - DEFAULT_SENSOR_PUBLISHER_TOPIC = 'franka_ros_interface/sensor' DYNAMIC_SKILL_WAIT_TIME = 0.3 DEFAULT_HFPC_FORCE_GAIN = [0.1] * 6 diff --git a/frankapy/franka_interface_common_definitions.py b/frankapy/franka_interface_common_definitions.py index 9bceb8c..70d660b 100644 --- a/frankapy/franka_interface_common_definitions.py +++ b/frankapy/franka_interface_common_definitions.py @@ -23,6 +23,7 @@ class SkillType: GripperSkill = _enum_auto('SkillType') ImpedanceControlSkill = _enum_auto('SkillType') JointPositionSkill = _enum_auto('SkillType') + JointVelocitySkill = _enum_auto('SkillType') class MetaSkillType: @@ -44,6 +45,7 @@ class TrajectoryGeneratorType: MinJerkPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') PassThroughForcePositionTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') PassThroughJointTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') + PassThroughJointVelocityTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') PassThroughPoseTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') PoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') QuaternionPoseDmpTrajectoryGenerator = _enum_auto('TrajectoryGeneratorType') @@ -79,6 +81,7 @@ class SkillStatus: RUNNING = _enum_auto('SkillStatus') FINISHED = _enum_auto('SkillStatus') VIRT_COLL_ERR = _enum_auto('SkillStatus') + FRANKA_ERR = _enum_auto('SkillStatus') class SensorDataMessageType: @@ -88,6 +91,7 @@ class SensorDataMessageType: FORCE_POSITION_GAINS = _enum_auto('SensorDataMessageType') JOINT_POSITION_VELOCITY = _enum_auto('SensorDataMessageType') JOINT_POSITION = _enum_auto('SensorDataMessageType') + JOINT_VELOCITY = _enum_auto('SensorDataMessageType') POSE_POSITION_VELOCITY = _enum_auto('SensorDataMessageType') POSE_POSITION = _enum_auto('SensorDataMessageType') SHOULD_TERMINATE = _enum_auto('SensorDataMessageType') diff --git a/frankapy/franka_interface_status_client.py b/frankapy/franka_interface_status_client.py new file mode 100644 index 0000000..9b75148 --- /dev/null +++ b/frankapy/franka_interface_status_client.py @@ -0,0 +1,28 @@ +import rclpy +from rclpy.node import Node +from franka_interface_msgs.msg import FrankaInterfaceStatus +from franka_interface_msgs.srv import GetCurrentFrankaInterfaceStatus + + +class FrankaInterfaceStatusClient(Node): + + def __init__(self, franka_interface_status_server_name='/get_current_franka_interface_status_server_node_1/franka_interface_status', offline=False): + super().__init__('franka_interface_status_client') + + self._offline = offline + if not self._offline: + self._franka_interface_status_client = self.create_client(GetCurrentFrankaInterfaceStatus, franka_interface_status_server_name) + while not self._franka_interface_status_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Get Current Robot State Service is not available, waiting again...') + self.req = GetCurrentFrankaInterfaceStatus.Request() + + def get_current_franka_interface_status(self): + if self._offline: + current_franka_interface_status = FrankaInterfaceStatus() + current_franka_interface_status.is_ready = True + current_franka_interface_status.is_fresh = True + return current_franka_interface_status + + self.future = self._franka_interface_status_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result().franka_interface_status \ No newline at end of file diff --git a/frankapy/franka_arm_state_client.py b/frankapy/franka_robot_state_client.py similarity index 69% rename from frankapy/franka_arm_state_client.py rename to frankapy/franka_robot_state_client.py index 81bae51..273e33c 100644 --- a/frankapy/franka_arm_state_client.py +++ b/frankapy/franka_robot_state_client.py @@ -1,22 +1,24 @@ import logging import numpy as np -import rospy -from franka_interface_msgs.srv import GetCurrentRobotStateCmd - -from .utils import franka_pose_to_rigid_transform +import rclpy +from rclpy.node import Node +from franka_interface_msgs.srv import GetCurrentRobotState -class FrankaArmStateClient: +from .utils import franka_pose_to_rigid_transform - def __init__(self, new_ros_node=True, robot_state_server_name='/get_current_robot_state_server_node_1/get_current_robot_state_server', offline=False): - if new_ros_node: - rospy.init_node('FrankaArmStateClient', anonymous=True) +class FrankaRobotStateClient(Node): + def __init__(self, robot_state_server_name='/get_current_robot_state_server_node_1/robot_state', offline=False): + super().__init__('franka_arm_state_client') + self._offline = offline if not self._offline: - rospy.wait_for_service(robot_state_server_name) - self._get_current_robot_state = rospy.ServiceProxy(robot_state_server_name, GetCurrentRobotStateCmd) + self._robot_state_client = self.create_client(GetCurrentRobotState, robot_state_server_name) + while not self._robot_state_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Get Current Robot State Service is not available, waiting again...') + self.req = GetCurrentRobotState.Request() def get_data(self): '''Get all fields of current robot data in a dict. @@ -25,7 +27,7 @@ def get_data(self): dict of robot state ''' if self._offline: - logging.warn('In offline mode - FrankaArmStateClient will return 0 values.') + logging.warn('In offline mode - FrankaRobotStateClient will return 0 values.') return { 'pose': franka_pose_to_rigid_transform(np.eye(4)), 'pose_desired': franka_pose_to_rigid_transform(np.eye(4)), @@ -39,19 +41,21 @@ def get_data(self): 'ee_force_torque': np.zeros(6) } - ros_data = self._get_current_robot_state().robot_state + self.future = self._robot_state_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + ros_data = self.future.result().robot_state data = { - 'pose': franka_pose_to_rigid_transform(ros_data.O_T_EE), - 'pose_desired': franka_pose_to_rigid_transform(ros_data.O_T_EE_d), - 'joint_torques': np.array(ros_data.tau_J), - 'joint_torques_derivative': np.array(ros_data.dtau_J), + 'pose': franka_pose_to_rigid_transform(ros_data.o_t_ee), + 'pose_desired': franka_pose_to_rigid_transform(ros_data.o_t_ee_d), + 'joint_torques': np.array(ros_data.tau_j), + 'joint_torques_derivative': np.array(ros_data.dtau_j), 'joints': np.array(ros_data.q), 'joints_desired': np.array(ros_data.q_d), 'joint_velocities': np.array(ros_data.dq), 'gripper_width': ros_data.gripper_width, 'gripper_is_grasped': ros_data.gripper_is_grasped, - 'ee_force_torque': np.array(ros_data.O_F_ext_hat_K) + 'ee_force_torque': np.array(ros_data.o_f_ext_hat_k) } return data diff --git a/frankapy/gripper_state_client.py b/frankapy/gripper_state_client.py new file mode 100644 index 0000000..eef9cd5 --- /dev/null +++ b/frankapy/gripper_state_client.py @@ -0,0 +1,26 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState +from franka_interface_msgs.srv import GetCurrentGripperState + + +class GripperStateClient(Node): + + def __init__(self, gripper_state_server_name='/get_current_gripper_state_server_node_1/gripper_state', offline=False): + super().__init__('gripper_state_client') + + self._offline = offline + if not self._offline: + self._gripper_state_client = self.create_client(GetCurrentGripperState, gripper_state_server_name) + while not self._gripper_state_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Get Current Gripper State Service is not available, waiting again...') + self.req = GetCurrentGripperState.Request() + + def get_current_gripper_state(self): + if self._offline: + current_gripper_state = JointState() + return current_gripper_state + + self.future = self._gripper_state_client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + return self.future.result().gripper_state \ No newline at end of file diff --git a/frankapy/proto/__init__.py b/frankapy/proto/__init__.py index 822ca4d..d360fe9 100644 --- a/frankapy/proto/__init__.py +++ b/frankapy/proto/__init__.py @@ -1,4 +1,5 @@ from .feedback_controller_params_msg_pb2 import * +from .result_msg_pb2 import * from .sensor_msg_pb2 import * from .termination_handler_params_msg_pb2 import * from .trajectory_generator_params_msg_pb2 import * \ No newline at end of file diff --git a/frankapy/proto/feedback_controller_params_msg_pb2.py b/frankapy/proto/feedback_controller_params_msg_pb2.py index 1bfeff7..5bae74e 100644 --- a/frankapy/proto/feedback_controller_params_msg_pb2.py +++ b/frankapy/proto/feedback_controller_params_msg_pb2.py @@ -1,10 +1,10 @@ # -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: feedback_controller_params_msg.proto - +"""Generated protocol buffer code.""" +from google.protobuf.internal import builder as _builder from google.protobuf import descriptor as _descriptor -from google.protobuf import message as _message -from google.protobuf import reflection as _reflection +from google.protobuf import descriptor_pool as _descriptor_pool from google.protobuf import symbol_database as _symbol_database # @@protoc_insertion_point(imports) @@ -13,282 +13,21 @@ -DESCRIPTOR = _descriptor.FileDescriptor( - name='feedback_controller_params_msg.proto', - package='', - syntax='proto2', - serialized_options=None, - serialized_pb=b'\n$feedback_controller_params_msg.proto\"p\n+CartesianImpedanceFeedbackControllerMessage\x12!\n\x19translational_stiffnesses\x18\x01 \x03(\x01\x12\x1e\n\x16rotational_stiffnesses\x18\x02 \x03(\x01\"q\n\"ForceAxisFeedbackControllerMessage\x12\x1f\n\x17translational_stiffness\x18\x01 \x02(\x01\x12\x1c\n\x14rotational_stiffness\x18\x02 \x02(\x01\x12\x0c\n\x04\x61xis\x18\x03 \x03(\x01\"K\n\'JointImpedanceFeedbackControllerMessage\x12\x0f\n\x07k_gains\x18\x01 \x03(\x01\x12\x0f\n\x07\x64_gains\x18\x02 \x03(\x01\"d\n*InternalImpedanceFeedbackControllerMessage\x12\x1c\n\x14\x63\x61rtesian_impedances\x18\x01 \x03(\x01\x12\x18\n\x10joint_impedances\x18\x02 \x03(\x01\"\xc0\x01\n&ForcePositionFeedbackControllerMessage\x12\x19\n\x11position_kps_cart\x18\x01 \x03(\x01\x12\x16\n\x0e\x66orce_kps_cart\x18\x02 \x03(\x01\x12\x1a\n\x12position_kps_joint\x18\x03 \x03(\x01\x12\x17\n\x0f\x66orce_kps_joint\x18\x04 \x03(\x01\x12\x11\n\tselection\x18\x05 \x03(\x01\x12\x1b\n\x13use_cartesian_gains\x18\x06 \x02(\x08' -) - - - - -_CARTESIANIMPEDANCEFEEDBACKCONTROLLERMESSAGE = _descriptor.Descriptor( - name='CartesianImpedanceFeedbackControllerMessage', - full_name='CartesianImpedanceFeedbackControllerMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='translational_stiffnesses', full_name='CartesianImpedanceFeedbackControllerMessage.translational_stiffnesses', index=0, - number=1, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='rotational_stiffnesses', full_name='CartesianImpedanceFeedbackControllerMessage.rotational_stiffnesses', index=1, - number=2, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=40, - serialized_end=152, -) - - -_FORCEAXISFEEDBACKCONTROLLERMESSAGE = _descriptor.Descriptor( - name='ForceAxisFeedbackControllerMessage', - full_name='ForceAxisFeedbackControllerMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='translational_stiffness', full_name='ForceAxisFeedbackControllerMessage.translational_stiffness', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='rotational_stiffness', full_name='ForceAxisFeedbackControllerMessage.rotational_stiffness', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='axis', full_name='ForceAxisFeedbackControllerMessage.axis', index=2, - number=3, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=154, - serialized_end=267, -) - - -_JOINTIMPEDANCEFEEDBACKCONTROLLERMESSAGE = _descriptor.Descriptor( - name='JointImpedanceFeedbackControllerMessage', - full_name='JointImpedanceFeedbackControllerMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='k_gains', full_name='JointImpedanceFeedbackControllerMessage.k_gains', index=0, - number=1, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='d_gains', full_name='JointImpedanceFeedbackControllerMessage.d_gains', index=1, - number=2, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=269, - serialized_end=344, -) - - -_INTERNALIMPEDANCEFEEDBACKCONTROLLERMESSAGE = _descriptor.Descriptor( - name='InternalImpedanceFeedbackControllerMessage', - full_name='InternalImpedanceFeedbackControllerMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='cartesian_impedances', full_name='InternalImpedanceFeedbackControllerMessage.cartesian_impedances', index=0, - number=1, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='joint_impedances', full_name='InternalImpedanceFeedbackControllerMessage.joint_impedances', index=1, - number=2, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=346, - serialized_end=446, -) - - -_FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE = _descriptor.Descriptor( - name='ForcePositionFeedbackControllerMessage', - full_name='ForcePositionFeedbackControllerMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='position_kps_cart', full_name='ForcePositionFeedbackControllerMessage.position_kps_cart', index=0, - number=1, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='force_kps_cart', full_name='ForcePositionFeedbackControllerMessage.force_kps_cart', index=1, - number=2, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='position_kps_joint', full_name='ForcePositionFeedbackControllerMessage.position_kps_joint', index=2, - number=3, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='force_kps_joint', full_name='ForcePositionFeedbackControllerMessage.force_kps_joint', index=3, - number=4, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='selection', full_name='ForcePositionFeedbackControllerMessage.selection', index=4, - number=5, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='use_cartesian_gains', full_name='ForcePositionFeedbackControllerMessage.use_cartesian_gains', index=5, - number=6, type=8, cpp_type=7, label=2, - has_default_value=False, default_value=False, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=449, - serialized_end=641, -) - -DESCRIPTOR.message_types_by_name['CartesianImpedanceFeedbackControllerMessage'] = _CARTESIANIMPEDANCEFEEDBACKCONTROLLERMESSAGE -DESCRIPTOR.message_types_by_name['ForceAxisFeedbackControllerMessage'] = _FORCEAXISFEEDBACKCONTROLLERMESSAGE -DESCRIPTOR.message_types_by_name['JointImpedanceFeedbackControllerMessage'] = _JOINTIMPEDANCEFEEDBACKCONTROLLERMESSAGE -DESCRIPTOR.message_types_by_name['InternalImpedanceFeedbackControllerMessage'] = _INTERNALIMPEDANCEFEEDBACKCONTROLLERMESSAGE -DESCRIPTOR.message_types_by_name['ForcePositionFeedbackControllerMessage'] = _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE -_sym_db.RegisterFileDescriptor(DESCRIPTOR) - -CartesianImpedanceFeedbackControllerMessage = _reflection.GeneratedProtocolMessageType('CartesianImpedanceFeedbackControllerMessage', (_message.Message,), { - 'DESCRIPTOR' : _CARTESIANIMPEDANCEFEEDBACKCONTROLLERMESSAGE, - '__module__' : 'feedback_controller_params_msg_pb2' - # @@protoc_insertion_point(class_scope:CartesianImpedanceFeedbackControllerMessage) - }) -_sym_db.RegisterMessage(CartesianImpedanceFeedbackControllerMessage) - -ForceAxisFeedbackControllerMessage = _reflection.GeneratedProtocolMessageType('ForceAxisFeedbackControllerMessage', (_message.Message,), { - 'DESCRIPTOR' : _FORCEAXISFEEDBACKCONTROLLERMESSAGE, - '__module__' : 'feedback_controller_params_msg_pb2' - # @@protoc_insertion_point(class_scope:ForceAxisFeedbackControllerMessage) - }) -_sym_db.RegisterMessage(ForceAxisFeedbackControllerMessage) - -JointImpedanceFeedbackControllerMessage = _reflection.GeneratedProtocolMessageType('JointImpedanceFeedbackControllerMessage', (_message.Message,), { - 'DESCRIPTOR' : _JOINTIMPEDANCEFEEDBACKCONTROLLERMESSAGE, - '__module__' : 'feedback_controller_params_msg_pb2' - # @@protoc_insertion_point(class_scope:JointImpedanceFeedbackControllerMessage) - }) -_sym_db.RegisterMessage(JointImpedanceFeedbackControllerMessage) - -InternalImpedanceFeedbackControllerMessage = _reflection.GeneratedProtocolMessageType('InternalImpedanceFeedbackControllerMessage', (_message.Message,), { - 'DESCRIPTOR' : _INTERNALIMPEDANCEFEEDBACKCONTROLLERMESSAGE, - '__module__' : 'feedback_controller_params_msg_pb2' - # @@protoc_insertion_point(class_scope:InternalImpedanceFeedbackControllerMessage) - }) -_sym_db.RegisterMessage(InternalImpedanceFeedbackControllerMessage) - -ForcePositionFeedbackControllerMessage = _reflection.GeneratedProtocolMessageType('ForcePositionFeedbackControllerMessage', (_message.Message,), { - 'DESCRIPTOR' : _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE, - '__module__' : 'feedback_controller_params_msg_pb2' - # @@protoc_insertion_point(class_scope:ForcePositionFeedbackControllerMessage) - }) -_sym_db.RegisterMessage(ForcePositionFeedbackControllerMessage) +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n$feedback_controller_params_msg.proto\"p\n+CartesianImpedanceFeedbackControllerMessage\x12!\n\x19translational_stiffnesses\x18\x01 \x03(\x01\x12\x1e\n\x16rotational_stiffnesses\x18\x02 \x03(\x01\"q\n\"ForceAxisFeedbackControllerMessage\x12\x1f\n\x17translational_stiffness\x18\x01 \x02(\x01\x12\x1c\n\x14rotational_stiffness\x18\x02 \x02(\x01\x12\x0c\n\x04\x61xis\x18\x03 \x03(\x01\"K\n\'JointImpedanceFeedbackControllerMessage\x12\x0f\n\x07k_gains\x18\x01 \x03(\x01\x12\x0f\n\x07\x64_gains\x18\x02 \x03(\x01\"d\n*InternalImpedanceFeedbackControllerMessage\x12\x1c\n\x14\x63\x61rtesian_impedances\x18\x01 \x03(\x01\x12\x18\n\x10joint_impedances\x18\x02 \x03(\x01\"\xc0\x01\n&ForcePositionFeedbackControllerMessage\x12\x19\n\x11position_kps_cart\x18\x01 \x03(\x01\x12\x16\n\x0e\x66orce_kps_cart\x18\x02 \x03(\x01\x12\x1a\n\x12position_kps_joint\x18\x03 \x03(\x01\x12\x17\n\x0f\x66orce_kps_joint\x18\x04 \x03(\x01\x12\x11\n\tselection\x18\x05 \x03(\x01\x12\x1b\n\x13use_cartesian_gains\x18\x06 \x02(\x08') +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'feedback_controller_params_msg_pb2', globals()) +if _descriptor._USE_C_DESCRIPTORS == False: + DESCRIPTOR._options = None + _CARTESIANIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_start=40 + _CARTESIANIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_end=152 + _FORCEAXISFEEDBACKCONTROLLERMESSAGE._serialized_start=154 + _FORCEAXISFEEDBACKCONTROLLERMESSAGE._serialized_end=267 + _JOINTIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_start=269 + _JOINTIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_end=344 + _INTERNALIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_start=346 + _INTERNALIMPEDANCEFEEDBACKCONTROLLERMESSAGE._serialized_end=446 + _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE._serialized_start=449 + _FORCEPOSITIONFEEDBACKCONTROLLERMESSAGE._serialized_end=641 # @@protoc_insertion_point(module_scope) diff --git a/frankapy/proto/result_msg.proto b/frankapy/proto/result_msg.proto new file mode 100644 index 0000000..8bc1b78 --- /dev/null +++ b/frankapy/proto/result_msg.proto @@ -0,0 +1,5 @@ +syntax = "proto2"; + +message ExecuteSkillResultMessage { + required uint32 skill_result = 1; +} \ No newline at end of file diff --git a/frankapy/proto/result_msg_pb2.py b/frankapy/proto/result_msg_pb2.py new file mode 100644 index 0000000..f0b01ec --- /dev/null +++ b/frankapy/proto/result_msg_pb2.py @@ -0,0 +1,25 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: result_msg.proto +"""Generated protocol buffer code.""" +from google.protobuf.internal import builder as _builder +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x10result_msg.proto\"1\n\x19\x45xecuteSkillResultMessage\x12\x14\n\x0cskill_result\x18\x01 \x02(\r') + +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'result_msg_pb2', globals()) +if _descriptor._USE_C_DESCRIPTORS == False: + + DESCRIPTOR._options = None + _EXECUTESKILLRESULTMESSAGE._serialized_start=20 + _EXECUTESKILLRESULTMESSAGE._serialized_end=69 +# @@protoc_insertion_point(module_scope) diff --git a/frankapy/proto/sensor_msg.proto b/frankapy/proto/sensor_msg.proto index ec79797..17324b2 100644 --- a/frankapy/proto/sensor_msg.proto +++ b/frankapy/proto/sensor_msg.proto @@ -31,6 +31,13 @@ message PosePositionVelocitySensorMessage { repeated double pose_velocities = 6; } +message JointVelocitySensorMessage { + required int32 id = 1; + required double timestamp = 2; + + repeated double joint_velocities = 3; +} + message JointPositionSensorMessage { required int32 id = 1; required double timestamp = 2; diff --git a/frankapy/proto/sensor_msg_pb2.py b/frankapy/proto/sensor_msg_pb2.py index 760030b..6828e80 100644 --- a/frankapy/proto/sensor_msg_pb2.py +++ b/frankapy/proto/sensor_msg_pb2.py @@ -1,10 +1,10 @@ # -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: sensor_msg.proto - +"""Generated protocol buffer code.""" +from google.protobuf.internal import builder as _builder from google.protobuf import descriptor as _descriptor -from google.protobuf import message as _message -from google.protobuf import reflection as _reflection +from google.protobuf import descriptor_pool as _descriptor_pool from google.protobuf import symbol_database as _symbol_database # @@protoc_insertion_point(imports) @@ -13,599 +13,31 @@ -DESCRIPTOR = _descriptor.FileDescriptor( - name='sensor_msg.proto', - package='', - syntax='proto2', - serialized_options=None, - serialized_pb=b'\n\x10sensor_msg.proto\"S\n\x0b\x42oundingBox\x12\x0c\n\x04name\x18\x01 \x02(\t\x12\n\n\x02id\x18\x02 \x02(\x05\x12\t\n\x01x\x18\x03 \x02(\x05\x12\t\n\x01y\x18\x04 \x02(\x05\x12\t\n\x01w\x18\x05 \x02(\x05\x12\t\n\x01h\x18\x06 \x02(\x05\"}\n\"JointPositionVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x0e\n\x06joints\x18\x04 \x03(\x01\x12\x12\n\njoint_vels\x18\x05 \x03(\x01\"\x97\x01\n!PosePositionVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x10\n\x08position\x18\x04 \x03(\x01\x12\x12\n\nquaternion\x18\x05 \x03(\x01\x12\x17\n\x0fpose_velocities\x18\x06 \x03(\x01\"K\n\x1aJointPositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x0e\n\x06joints\x18\x03 \x03(\x01\"`\n\x19PosePositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x10\n\x08position\x18\x03 \x03(\x01\x12\x12\n\nquaternion\x18\x04 \x03(\x01\"K\n\x1cShouldTerminateSensorMessage\x12\x11\n\ttimestamp\x18\x01 \x02(\x01\x12\x18\n\x10should_terminate\x18\x02 \x02(\x08\"\x83\x01\n\x1f\x43\x61rtesianImpedanceSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12!\n\x19translational_stiffnesses\x18\x03 \x03(\x01\x12\x1e\n\x16rotational_stiffnesses\x18\x04 \x03(\x01\"n\n\x1a\x46orcePositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x0c\n\x04pose\x18\x04 \x03(\x01\x12\r\n\x05\x66orce\x18\x05 \x03(\x01\"\xc0\x01\n$ForcePositionControllerSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x19\n\x11position_kps_cart\x18\x03 \x03(\x01\x12\x16\n\x0e\x66orce_kps_cart\x18\x04 \x03(\x01\x12\x1a\n\x12position_kps_joint\x18\x05 \x03(\x01\x12\x17\n\x0f\x66orce_kps_joint\x18\x06 \x03(\x01\x12\x11\n\tselection\x18\x07 \x03(\x01' -) - - - - -_BOUNDINGBOX = _descriptor.Descriptor( - name='BoundingBox', - full_name='BoundingBox', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='name', full_name='BoundingBox.name', index=0, - number=1, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=b"".decode('utf-8'), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='id', full_name='BoundingBox.id', index=1, - number=2, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='x', full_name='BoundingBox.x', index=2, - number=3, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='y', full_name='BoundingBox.y', index=3, - number=4, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='w', full_name='BoundingBox.w', index=4, - number=5, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='h', full_name='BoundingBox.h', index=5, - number=6, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=20, - serialized_end=103, -) - - -_JOINTPOSITIONVELOCITYSENSORMESSAGE = _descriptor.Descriptor( - name='JointPositionVelocitySensorMessage', - full_name='JointPositionVelocitySensorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='id', full_name='JointPositionVelocitySensorMessage.id', index=0, - number=1, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='timestamp', full_name='JointPositionVelocitySensorMessage.timestamp', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='seg_run_time', full_name='JointPositionVelocitySensorMessage.seg_run_time', index=2, - number=3, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='joints', full_name='JointPositionVelocitySensorMessage.joints', index=3, - number=4, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='joint_vels', full_name='JointPositionVelocitySensorMessage.joint_vels', index=4, - number=5, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=105, - serialized_end=230, -) - - -_POSEPOSITIONVELOCITYSENSORMESSAGE = _descriptor.Descriptor( - name='PosePositionVelocitySensorMessage', - full_name='PosePositionVelocitySensorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='id', full_name='PosePositionVelocitySensorMessage.id', index=0, - number=1, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='timestamp', full_name='PosePositionVelocitySensorMessage.timestamp', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='seg_run_time', full_name='PosePositionVelocitySensorMessage.seg_run_time', index=2, - number=3, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='position', full_name='PosePositionVelocitySensorMessage.position', index=3, - number=4, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='quaternion', full_name='PosePositionVelocitySensorMessage.quaternion', index=4, - number=5, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='pose_velocities', full_name='PosePositionVelocitySensorMessage.pose_velocities', index=5, - number=6, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=233, - serialized_end=384, -) - - -_JOINTPOSITIONSENSORMESSAGE = _descriptor.Descriptor( - name='JointPositionSensorMessage', - full_name='JointPositionSensorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='id', full_name='JointPositionSensorMessage.id', index=0, - number=1, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='timestamp', full_name='JointPositionSensorMessage.timestamp', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='joints', full_name='JointPositionSensorMessage.joints', index=2, - number=3, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=386, - serialized_end=461, -) - - -_POSEPOSITIONSENSORMESSAGE = _descriptor.Descriptor( - name='PosePositionSensorMessage', - full_name='PosePositionSensorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='id', full_name='PosePositionSensorMessage.id', index=0, - number=1, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='timestamp', full_name='PosePositionSensorMessage.timestamp', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='position', full_name='PosePositionSensorMessage.position', index=2, - number=3, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='quaternion', full_name='PosePositionSensorMessage.quaternion', index=3, - number=4, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=463, - serialized_end=559, -) - - -_SHOULDTERMINATESENSORMESSAGE = _descriptor.Descriptor( - name='ShouldTerminateSensorMessage', - full_name='ShouldTerminateSensorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='timestamp', full_name='ShouldTerminateSensorMessage.timestamp', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='should_terminate', full_name='ShouldTerminateSensorMessage.should_terminate', index=1, - number=2, type=8, cpp_type=7, label=2, - has_default_value=False, default_value=False, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=561, - serialized_end=636, -) - - -_CARTESIANIMPEDANCESENSORMESSAGE = _descriptor.Descriptor( - name='CartesianImpedanceSensorMessage', - full_name='CartesianImpedanceSensorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='id', full_name='CartesianImpedanceSensorMessage.id', index=0, - number=1, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='timestamp', full_name='CartesianImpedanceSensorMessage.timestamp', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='translational_stiffnesses', full_name='CartesianImpedanceSensorMessage.translational_stiffnesses', index=2, - number=3, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='rotational_stiffnesses', full_name='CartesianImpedanceSensorMessage.rotational_stiffnesses', index=3, - number=4, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=639, - serialized_end=770, -) - - -_FORCEPOSITIONSENSORMESSAGE = _descriptor.Descriptor( - name='ForcePositionSensorMessage', - full_name='ForcePositionSensorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='id', full_name='ForcePositionSensorMessage.id', index=0, - number=1, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='timestamp', full_name='ForcePositionSensorMessage.timestamp', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='seg_run_time', full_name='ForcePositionSensorMessage.seg_run_time', index=2, - number=3, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='pose', full_name='ForcePositionSensorMessage.pose', index=3, - number=4, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='force', full_name='ForcePositionSensorMessage.force', index=4, - number=5, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=772, - serialized_end=882, -) - - -_FORCEPOSITIONCONTROLLERSENSORMESSAGE = _descriptor.Descriptor( - name='ForcePositionControllerSensorMessage', - full_name='ForcePositionControllerSensorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='id', full_name='ForcePositionControllerSensorMessage.id', index=0, - number=1, type=5, cpp_type=1, label=2, - has_default_value=False, default_value=0, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='timestamp', full_name='ForcePositionControllerSensorMessage.timestamp', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='position_kps_cart', full_name='ForcePositionControllerSensorMessage.position_kps_cart', index=2, - number=3, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='force_kps_cart', full_name='ForcePositionControllerSensorMessage.force_kps_cart', index=3, - number=4, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='position_kps_joint', full_name='ForcePositionControllerSensorMessage.position_kps_joint', index=4, - number=5, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='force_kps_joint', full_name='ForcePositionControllerSensorMessage.force_kps_joint', index=5, - number=6, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='selection', full_name='ForcePositionControllerSensorMessage.selection', index=6, - number=7, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=885, - serialized_end=1077, -) - -DESCRIPTOR.message_types_by_name['BoundingBox'] = _BOUNDINGBOX -DESCRIPTOR.message_types_by_name['JointPositionVelocitySensorMessage'] = _JOINTPOSITIONVELOCITYSENSORMESSAGE -DESCRIPTOR.message_types_by_name['PosePositionVelocitySensorMessage'] = _POSEPOSITIONVELOCITYSENSORMESSAGE -DESCRIPTOR.message_types_by_name['JointPositionSensorMessage'] = _JOINTPOSITIONSENSORMESSAGE -DESCRIPTOR.message_types_by_name['PosePositionSensorMessage'] = _POSEPOSITIONSENSORMESSAGE -DESCRIPTOR.message_types_by_name['ShouldTerminateSensorMessage'] = _SHOULDTERMINATESENSORMESSAGE -DESCRIPTOR.message_types_by_name['CartesianImpedanceSensorMessage'] = _CARTESIANIMPEDANCESENSORMESSAGE -DESCRIPTOR.message_types_by_name['ForcePositionSensorMessage'] = _FORCEPOSITIONSENSORMESSAGE -DESCRIPTOR.message_types_by_name['ForcePositionControllerSensorMessage'] = _FORCEPOSITIONCONTROLLERSENSORMESSAGE -_sym_db.RegisterFileDescriptor(DESCRIPTOR) - -BoundingBox = _reflection.GeneratedProtocolMessageType('BoundingBox', (_message.Message,), { - 'DESCRIPTOR' : _BOUNDINGBOX, - '__module__' : 'sensor_msg_pb2' - # @@protoc_insertion_point(class_scope:BoundingBox) - }) -_sym_db.RegisterMessage(BoundingBox) - -JointPositionVelocitySensorMessage = _reflection.GeneratedProtocolMessageType('JointPositionVelocitySensorMessage', (_message.Message,), { - 'DESCRIPTOR' : _JOINTPOSITIONVELOCITYSENSORMESSAGE, - '__module__' : 'sensor_msg_pb2' - # @@protoc_insertion_point(class_scope:JointPositionVelocitySensorMessage) - }) -_sym_db.RegisterMessage(JointPositionVelocitySensorMessage) - -PosePositionVelocitySensorMessage = _reflection.GeneratedProtocolMessageType('PosePositionVelocitySensorMessage', (_message.Message,), { - 'DESCRIPTOR' : _POSEPOSITIONVELOCITYSENSORMESSAGE, - '__module__' : 'sensor_msg_pb2' - # @@protoc_insertion_point(class_scope:PosePositionVelocitySensorMessage) - }) -_sym_db.RegisterMessage(PosePositionVelocitySensorMessage) - -JointPositionSensorMessage = _reflection.GeneratedProtocolMessageType('JointPositionSensorMessage', (_message.Message,), { - 'DESCRIPTOR' : _JOINTPOSITIONSENSORMESSAGE, - '__module__' : 'sensor_msg_pb2' - # @@protoc_insertion_point(class_scope:JointPositionSensorMessage) - }) -_sym_db.RegisterMessage(JointPositionSensorMessage) - -PosePositionSensorMessage = _reflection.GeneratedProtocolMessageType('PosePositionSensorMessage', (_message.Message,), { - 'DESCRIPTOR' : _POSEPOSITIONSENSORMESSAGE, - '__module__' : 'sensor_msg_pb2' - # @@protoc_insertion_point(class_scope:PosePositionSensorMessage) - }) -_sym_db.RegisterMessage(PosePositionSensorMessage) - -ShouldTerminateSensorMessage = _reflection.GeneratedProtocolMessageType('ShouldTerminateSensorMessage', (_message.Message,), { - 'DESCRIPTOR' : _SHOULDTERMINATESENSORMESSAGE, - '__module__' : 'sensor_msg_pb2' - # @@protoc_insertion_point(class_scope:ShouldTerminateSensorMessage) - }) -_sym_db.RegisterMessage(ShouldTerminateSensorMessage) - -CartesianImpedanceSensorMessage = _reflection.GeneratedProtocolMessageType('CartesianImpedanceSensorMessage', (_message.Message,), { - 'DESCRIPTOR' : _CARTESIANIMPEDANCESENSORMESSAGE, - '__module__' : 'sensor_msg_pb2' - # @@protoc_insertion_point(class_scope:CartesianImpedanceSensorMessage) - }) -_sym_db.RegisterMessage(CartesianImpedanceSensorMessage) - -ForcePositionSensorMessage = _reflection.GeneratedProtocolMessageType('ForcePositionSensorMessage', (_message.Message,), { - 'DESCRIPTOR' : _FORCEPOSITIONSENSORMESSAGE, - '__module__' : 'sensor_msg_pb2' - # @@protoc_insertion_point(class_scope:ForcePositionSensorMessage) - }) -_sym_db.RegisterMessage(ForcePositionSensorMessage) - -ForcePositionControllerSensorMessage = _reflection.GeneratedProtocolMessageType('ForcePositionControllerSensorMessage', (_message.Message,), { - 'DESCRIPTOR' : _FORCEPOSITIONCONTROLLERSENSORMESSAGE, - '__module__' : 'sensor_msg_pb2' - # @@protoc_insertion_point(class_scope:ForcePositionControllerSensorMessage) - }) -_sym_db.RegisterMessage(ForcePositionControllerSensorMessage) - - +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x10sensor_msg.proto\"S\n\x0b\x42oundingBox\x12\x0c\n\x04name\x18\x01 \x02(\t\x12\n\n\x02id\x18\x02 \x02(\x05\x12\t\n\x01x\x18\x03 \x02(\x05\x12\t\n\x01y\x18\x04 \x02(\x05\x12\t\n\x01w\x18\x05 \x02(\x05\x12\t\n\x01h\x18\x06 \x02(\x05\"}\n\"JointPositionVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x0e\n\x06joints\x18\x04 \x03(\x01\x12\x12\n\njoint_vels\x18\x05 \x03(\x01\"\x97\x01\n!PosePositionVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x10\n\x08position\x18\x04 \x03(\x01\x12\x12\n\nquaternion\x18\x05 \x03(\x01\x12\x17\n\x0fpose_velocities\x18\x06 \x03(\x01\"U\n\x1aJointVelocitySensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x18\n\x10joint_velocities\x18\x03 \x03(\x01\"K\n\x1aJointPositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x0e\n\x06joints\x18\x03 \x03(\x01\"`\n\x19PosePositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x10\n\x08position\x18\x03 \x03(\x01\x12\x12\n\nquaternion\x18\x04 \x03(\x01\"K\n\x1cShouldTerminateSensorMessage\x12\x11\n\ttimestamp\x18\x01 \x02(\x01\x12\x18\n\x10should_terminate\x18\x02 \x02(\x08\"\x83\x01\n\x1f\x43\x61rtesianImpedanceSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12!\n\x19translational_stiffnesses\x18\x03 \x03(\x01\x12\x1e\n\x16rotational_stiffnesses\x18\x04 \x03(\x01\"n\n\x1a\x46orcePositionSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x14\n\x0cseg_run_time\x18\x03 \x02(\x01\x12\x0c\n\x04pose\x18\x04 \x03(\x01\x12\r\n\x05\x66orce\x18\x05 \x03(\x01\"\xc0\x01\n$ForcePositionControllerSensorMessage\x12\n\n\x02id\x18\x01 \x02(\x05\x12\x11\n\ttimestamp\x18\x02 \x02(\x01\x12\x19\n\x11position_kps_cart\x18\x03 \x03(\x01\x12\x16\n\x0e\x66orce_kps_cart\x18\x04 \x03(\x01\x12\x1a\n\x12position_kps_joint\x18\x05 \x03(\x01\x12\x17\n\x0f\x66orce_kps_joint\x18\x06 \x03(\x01\x12\x11\n\tselection\x18\x07 \x03(\x01') + +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'sensor_msg_pb2', globals()) +if _descriptor._USE_C_DESCRIPTORS == False: + + DESCRIPTOR._options = None + _BOUNDINGBOX._serialized_start=20 + _BOUNDINGBOX._serialized_end=103 + _JOINTPOSITIONVELOCITYSENSORMESSAGE._serialized_start=105 + _JOINTPOSITIONVELOCITYSENSORMESSAGE._serialized_end=230 + _POSEPOSITIONVELOCITYSENSORMESSAGE._serialized_start=233 + _POSEPOSITIONVELOCITYSENSORMESSAGE._serialized_end=384 + _JOINTVELOCITYSENSORMESSAGE._serialized_start=386 + _JOINTVELOCITYSENSORMESSAGE._serialized_end=471 + _JOINTPOSITIONSENSORMESSAGE._serialized_start=473 + _JOINTPOSITIONSENSORMESSAGE._serialized_end=548 + _POSEPOSITIONSENSORMESSAGE._serialized_start=550 + _POSEPOSITIONSENSORMESSAGE._serialized_end=646 + _SHOULDTERMINATESENSORMESSAGE._serialized_start=648 + _SHOULDTERMINATESENSORMESSAGE._serialized_end=723 + _CARTESIANIMPEDANCESENSORMESSAGE._serialized_start=726 + _CARTESIANIMPEDANCESENSORMESSAGE._serialized_end=857 + _FORCEPOSITIONSENSORMESSAGE._serialized_start=859 + _FORCEPOSITIONSENSORMESSAGE._serialized_end=969 + _FORCEPOSITIONCONTROLLERSENSORMESSAGE._serialized_start=972 + _FORCEPOSITIONCONTROLLERSENSORMESSAGE._serialized_end=1164 # @@protoc_insertion_point(module_scope) diff --git a/frankapy/proto/termination_handler_params_msg_pb2.py b/frankapy/proto/termination_handler_params_msg_pb2.py index f9367b2..0733e4f 100644 --- a/frankapy/proto/termination_handler_params_msg_pb2.py +++ b/frankapy/proto/termination_handler_params_msg_pb2.py @@ -1,10 +1,10 @@ # -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: termination_handler_params_msg.proto - +"""Generated protocol buffer code.""" +from google.protobuf.internal import builder as _builder from google.protobuf import descriptor as _descriptor -from google.protobuf import message as _message -from google.protobuf import reflection as _reflection +from google.protobuf import descriptor_pool as _descriptor_pool from google.protobuf import symbol_database as _symbol_database # @@protoc_insertion_point(imports) @@ -13,208 +13,19 @@ -DESCRIPTOR = _descriptor.FileDescriptor( - name='termination_handler_params_msg.proto', - package='', - syntax='proto2', - serialized_options=None, - serialized_pb=b'\n$termination_handler_params_msg.proto\"l\n ContactTerminationHandlerMessage\x12\x13\n\x0b\x62uffer_time\x18\x01 \x02(\x01\x12\x18\n\x10\x66orce_thresholds\x18\x02 \x03(\x01\x12\x19\n\x11torque_thresholds\x18\x03 \x03(\x01\"F\n\x15JointThresholdMessage\x12\x13\n\x0b\x62uffer_time\x18\x01 \x02(\x01\x12\x18\n\x10joint_thresholds\x18\x02 \x03(\x01\"h\n\x14PoseThresholdMessage\x12\x13\n\x0b\x62uffer_time\x18\x01 \x02(\x01\x12\x1b\n\x13position_thresholds\x18\x02 \x03(\x01\x12\x1e\n\x16orientation_thresholds\x18\x03 \x03(\x01\"4\n\x1dTimeTerminationHandlerMessage\x12\x13\n\x0b\x62uffer_time\x18\x01 \x02(\x01' -) - - - - -_CONTACTTERMINATIONHANDLERMESSAGE = _descriptor.Descriptor( - name='ContactTerminationHandlerMessage', - full_name='ContactTerminationHandlerMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='buffer_time', full_name='ContactTerminationHandlerMessage.buffer_time', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='force_thresholds', full_name='ContactTerminationHandlerMessage.force_thresholds', index=1, - number=2, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='torque_thresholds', full_name='ContactTerminationHandlerMessage.torque_thresholds', index=2, - number=3, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=40, - serialized_end=148, -) - - -_JOINTTHRESHOLDMESSAGE = _descriptor.Descriptor( - name='JointThresholdMessage', - full_name='JointThresholdMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='buffer_time', full_name='JointThresholdMessage.buffer_time', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='joint_thresholds', full_name='JointThresholdMessage.joint_thresholds', index=1, - number=2, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=150, - serialized_end=220, -) - - -_POSETHRESHOLDMESSAGE = _descriptor.Descriptor( - name='PoseThresholdMessage', - full_name='PoseThresholdMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='buffer_time', full_name='PoseThresholdMessage.buffer_time', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='position_thresholds', full_name='PoseThresholdMessage.position_thresholds', index=1, - number=2, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='orientation_thresholds', full_name='PoseThresholdMessage.orientation_thresholds', index=2, - number=3, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=222, - serialized_end=326, -) - - -_TIMETERMINATIONHANDLERMESSAGE = _descriptor.Descriptor( - name='TimeTerminationHandlerMessage', - full_name='TimeTerminationHandlerMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='buffer_time', full_name='TimeTerminationHandlerMessage.buffer_time', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=328, - serialized_end=380, -) - -DESCRIPTOR.message_types_by_name['ContactTerminationHandlerMessage'] = _CONTACTTERMINATIONHANDLERMESSAGE -DESCRIPTOR.message_types_by_name['JointThresholdMessage'] = _JOINTTHRESHOLDMESSAGE -DESCRIPTOR.message_types_by_name['PoseThresholdMessage'] = _POSETHRESHOLDMESSAGE -DESCRIPTOR.message_types_by_name['TimeTerminationHandlerMessage'] = _TIMETERMINATIONHANDLERMESSAGE -_sym_db.RegisterFileDescriptor(DESCRIPTOR) - -ContactTerminationHandlerMessage = _reflection.GeneratedProtocolMessageType('ContactTerminationHandlerMessage', (_message.Message,), { - 'DESCRIPTOR' : _CONTACTTERMINATIONHANDLERMESSAGE, - '__module__' : 'termination_handler_params_msg_pb2' - # @@protoc_insertion_point(class_scope:ContactTerminationHandlerMessage) - }) -_sym_db.RegisterMessage(ContactTerminationHandlerMessage) - -JointThresholdMessage = _reflection.GeneratedProtocolMessageType('JointThresholdMessage', (_message.Message,), { - 'DESCRIPTOR' : _JOINTTHRESHOLDMESSAGE, - '__module__' : 'termination_handler_params_msg_pb2' - # @@protoc_insertion_point(class_scope:JointThresholdMessage) - }) -_sym_db.RegisterMessage(JointThresholdMessage) - -PoseThresholdMessage = _reflection.GeneratedProtocolMessageType('PoseThresholdMessage', (_message.Message,), { - 'DESCRIPTOR' : _POSETHRESHOLDMESSAGE, - '__module__' : 'termination_handler_params_msg_pb2' - # @@protoc_insertion_point(class_scope:PoseThresholdMessage) - }) -_sym_db.RegisterMessage(PoseThresholdMessage) - -TimeTerminationHandlerMessage = _reflection.GeneratedProtocolMessageType('TimeTerminationHandlerMessage', (_message.Message,), { - 'DESCRIPTOR' : _TIMETERMINATIONHANDLERMESSAGE, - '__module__' : 'termination_handler_params_msg_pb2' - # @@protoc_insertion_point(class_scope:TimeTerminationHandlerMessage) - }) -_sym_db.RegisterMessage(TimeTerminationHandlerMessage) +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n$termination_handler_params_msg.proto\"l\n ContactTerminationHandlerMessage\x12\x13\n\x0b\x62uffer_time\x18\x01 \x02(\x01\x12\x18\n\x10\x66orce_thresholds\x18\x02 \x03(\x01\x12\x19\n\x11torque_thresholds\x18\x03 \x03(\x01\"F\n\x15JointThresholdMessage\x12\x13\n\x0b\x62uffer_time\x18\x01 \x02(\x01\x12\x18\n\x10joint_thresholds\x18\x02 \x03(\x01\"h\n\x14PoseThresholdMessage\x12\x13\n\x0b\x62uffer_time\x18\x01 \x02(\x01\x12\x1b\n\x13position_thresholds\x18\x02 \x03(\x01\x12\x1e\n\x16orientation_thresholds\x18\x03 \x03(\x01\"4\n\x1dTimeTerminationHandlerMessage\x12\x13\n\x0b\x62uffer_time\x18\x01 \x02(\x01') +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'termination_handler_params_msg_pb2', globals()) +if _descriptor._USE_C_DESCRIPTORS == False: + DESCRIPTOR._options = None + _CONTACTTERMINATIONHANDLERMESSAGE._serialized_start=40 + _CONTACTTERMINATIONHANDLERMESSAGE._serialized_end=148 + _JOINTTHRESHOLDMESSAGE._serialized_start=150 + _JOINTTHRESHOLDMESSAGE._serialized_end=220 + _POSETHRESHOLDMESSAGE._serialized_start=222 + _POSETHRESHOLDMESSAGE._serialized_end=326 + _TIMETERMINATIONHANDLERMESSAGE._serialized_start=328 + _TIMETERMINATIONHANDLERMESSAGE._serialized_end=380 # @@protoc_insertion_point(module_scope) diff --git a/frankapy/proto/trajectory_generator_params_msg.proto b/frankapy/proto/trajectory_generator_params_msg.proto index 5f76170..52055f5 100644 --- a/frankapy/proto/trajectory_generator_params_msg.proto +++ b/frankapy/proto/trajectory_generator_params_msg.proto @@ -23,6 +23,13 @@ message JointTrajectoryGeneratorMessage { repeated double joints = 2; } +message JointVelocityTrajectoryGeneratorMessage { + required double run_time = 1; + + repeated double joint_velocities = 2; + repeated double joint_accelerations = 3; +} + message PoseTrajectoryGeneratorMessage { required double run_time = 1; @@ -55,7 +62,7 @@ message PoseDMPTrajectoryGeneratorMessage { required double beta = 7; required double num_basis = 8; required double num_sensor_values = 9; - + repeated double basis_mean = 10; repeated double basis_std = 11; repeated double weights = 12; diff --git a/frankapy/proto/trajectory_generator_params_msg_pb2.py b/frankapy/proto/trajectory_generator_params_msg_pb2.py index 72f8ea7..8fcf67f 100644 --- a/frankapy/proto/trajectory_generator_params_msg_pb2.py +++ b/frankapy/proto/trajectory_generator_params_msg_pb2.py @@ -1,10 +1,10 @@ # -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: trajectory_generator_params_msg.proto - +"""Generated protocol buffer code.""" +from google.protobuf.internal import builder as _builder from google.protobuf import descriptor as _descriptor -from google.protobuf import message as _message -from google.protobuf import reflection as _reflection +from google.protobuf import descriptor_pool as _descriptor_pool from google.protobuf import symbol_database as _symbol_database # @@protoc_insertion_point(imports) @@ -13,728 +13,29 @@ -DESCRIPTOR = _descriptor.FileDescriptor( - name='trajectory_generator_params_msg.proto', - package='', - syntax='proto2', - serialized_options=None, - serialized_pb=b'\n%trajectory_generator_params_msg.proto\"_\n!GripperTrajectoryGeneratorMessage\x12\r\n\x05grasp\x18\x01 \x02(\x08\x12\r\n\x05width\x18\x02 \x02(\x01\x12\r\n\x05speed\x18\x03 \x02(\x01\x12\r\n\x05\x66orce\x18\x04 \x02(\x01\"\x8c\x01\n!ImpulseTrajectoryGeneratorMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01\x12\x10\n\x08\x61\x63\x63_time\x18\x02 \x02(\x01\x12\x11\n\tmax_trans\x18\x03 \x02(\x01\x12\x0f\n\x07max_rot\x18\x04 \x02(\x01\x12\x0e\n\x06\x66orces\x18\x05 \x03(\x01\x12\x0f\n\x07torques\x18\x06 \x03(\x01\"C\n\x1fJointTrajectoryGeneratorMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01\x12\x0e\n\x06joints\x18\x02 \x03(\x01\"f\n\x1ePoseTrajectoryGeneratorMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01\x12\x10\n\x08position\x18\x02 \x03(\x01\x12\x12\n\nquaternion\x18\x03 \x03(\x01\x12\x0c\n\x04pose\x18\x04 \x03(\x01\"\xe5\x01\n\"JointDMPTrajectoryGeneratorMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01\x12\x0b\n\x03tau\x18\x02 \x02(\x01\x12\r\n\x05\x61lpha\x18\x03 \x02(\x01\x12\x0c\n\x04\x62\x65ta\x18\x04 \x02(\x01\x12\x11\n\tnum_basis\x18\x05 \x02(\x01\x12\x19\n\x11num_sensor_values\x18\x06 \x02(\x01\x12\x12\n\nbasis_mean\x18\x07 \x03(\x01\x12\x11\n\tbasis_std\x18\x08 \x03(\x01\x12\x0f\n\x07weights\x18\t \x03(\x01\x12\x1d\n\x15initial_sensor_values\x18\n \x03(\x01\"\xa7\x02\n!PoseDMPTrajectoryGeneratorMessage\x12\x18\n\x10orientation_only\x18\x01 \x02(\x08\x12\x15\n\rposition_only\x18\x02 \x02(\x08\x12\x10\n\x08\x65\x65_frame\x18\x03 \x02(\x08\x12\x10\n\x08run_time\x18\x04 \x02(\x01\x12\x0b\n\x03tau\x18\x05 \x02(\x01\x12\r\n\x05\x61lpha\x18\x06 \x02(\x01\x12\x0c\n\x04\x62\x65ta\x18\x07 \x02(\x01\x12\x11\n\tnum_basis\x18\x08 \x02(\x01\x12\x19\n\x11num_sensor_values\x18\t \x02(\x01\x12\x12\n\nbasis_mean\x18\n \x03(\x01\x12\x11\n\tbasis_std\x18\x0b \x03(\x01\x12\x0f\n\x07weights\x18\x0c \x03(\x01\x12\x1d\n\x15initial_sensor_values\x18\r \x03(\x01\"\xec\x04\n+QuaternionPoseDMPTrajectoryGeneratorMessage\x12\x10\n\x08\x65\x65_frame\x18\x01 \x02(\x08\x12\x10\n\x08run_time\x18\x02 \x02(\x01\x12\x0f\n\x07tau_pos\x18\x03 \x02(\x01\x12\x11\n\talpha_pos\x18\x04 \x02(\x01\x12\x10\n\x08\x62\x65ta_pos\x18\x05 \x02(\x01\x12\x10\n\x08tau_quat\x18\x06 \x02(\x01\x12\x12\n\nalpha_quat\x18\x07 \x02(\x01\x12\x11\n\tbeta_quat\x18\x08 \x02(\x01\x12\x15\n\rnum_basis_pos\x18\t \x02(\x01\x12\x1d\n\x15num_sensor_values_pos\x18\n \x02(\x01\x12\x16\n\x0enum_basis_quat\x18\x0b \x02(\x01\x12\x1e\n\x16num_sensor_values_quat\x18\x0c \x02(\x01\x12\x16\n\x0epos_basis_mean\x18\r \x03(\x01\x12\x15\n\rpos_basis_std\x18\x0e \x03(\x01\x12\x13\n\x0bpos_weights\x18\x0f \x03(\x01\x12!\n\x19pos_initial_sensor_values\x18\x10 \x03(\x01\x12\x17\n\x0fquat_basis_mean\x18\x11 \x03(\x01\x12\x16\n\x0equat_basis_std\x18\x12 \x03(\x01\x12\x14\n\x0cquat_weights\x18\x13 \x03(\x01\x12\"\n\x1aquat_initial_sensor_values\x18\x14 \x03(\x01\x12\x16\n\x0egoal_time_quat\x18\x15 \x02(\x01\x12\x13\n\x0bgoal_quat_w\x18\x16 \x02(\x01\x12\x13\n\x0bgoal_quat_x\x18\x17 \x02(\x01\x12\x13\n\x0bgoal_quat_y\x18\x18 \x02(\x01\x12\x13\n\x0bgoal_quat_z\x18\x19 \x02(\x01\"\"\n\x0eRunTimeMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01' -) - - - - -_GRIPPERTRAJECTORYGENERATORMESSAGE = _descriptor.Descriptor( - name='GripperTrajectoryGeneratorMessage', - full_name='GripperTrajectoryGeneratorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='grasp', full_name='GripperTrajectoryGeneratorMessage.grasp', index=0, - number=1, type=8, cpp_type=7, label=2, - has_default_value=False, default_value=False, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='width', full_name='GripperTrajectoryGeneratorMessage.width', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='speed', full_name='GripperTrajectoryGeneratorMessage.speed', index=2, - number=3, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='force', full_name='GripperTrajectoryGeneratorMessage.force', index=3, - number=4, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=41, - serialized_end=136, -) - - -_IMPULSETRAJECTORYGENERATORMESSAGE = _descriptor.Descriptor( - name='ImpulseTrajectoryGeneratorMessage', - full_name='ImpulseTrajectoryGeneratorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='run_time', full_name='ImpulseTrajectoryGeneratorMessage.run_time', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='acc_time', full_name='ImpulseTrajectoryGeneratorMessage.acc_time', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='max_trans', full_name='ImpulseTrajectoryGeneratorMessage.max_trans', index=2, - number=3, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='max_rot', full_name='ImpulseTrajectoryGeneratorMessage.max_rot', index=3, - number=4, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='forces', full_name='ImpulseTrajectoryGeneratorMessage.forces', index=4, - number=5, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='torques', full_name='ImpulseTrajectoryGeneratorMessage.torques', index=5, - number=6, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=139, - serialized_end=279, -) - - -_JOINTTRAJECTORYGENERATORMESSAGE = _descriptor.Descriptor( - name='JointTrajectoryGeneratorMessage', - full_name='JointTrajectoryGeneratorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='run_time', full_name='JointTrajectoryGeneratorMessage.run_time', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='joints', full_name='JointTrajectoryGeneratorMessage.joints', index=1, - number=2, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=281, - serialized_end=348, -) - - -_POSETRAJECTORYGENERATORMESSAGE = _descriptor.Descriptor( - name='PoseTrajectoryGeneratorMessage', - full_name='PoseTrajectoryGeneratorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='run_time', full_name='PoseTrajectoryGeneratorMessage.run_time', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='position', full_name='PoseTrajectoryGeneratorMessage.position', index=1, - number=2, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='quaternion', full_name='PoseTrajectoryGeneratorMessage.quaternion', index=2, - number=3, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='pose', full_name='PoseTrajectoryGeneratorMessage.pose', index=3, - number=4, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=350, - serialized_end=452, -) - - -_JOINTDMPTRAJECTORYGENERATORMESSAGE = _descriptor.Descriptor( - name='JointDMPTrajectoryGeneratorMessage', - full_name='JointDMPTrajectoryGeneratorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='run_time', full_name='JointDMPTrajectoryGeneratorMessage.run_time', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='tau', full_name='JointDMPTrajectoryGeneratorMessage.tau', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='alpha', full_name='JointDMPTrajectoryGeneratorMessage.alpha', index=2, - number=3, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='beta', full_name='JointDMPTrajectoryGeneratorMessage.beta', index=3, - number=4, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='num_basis', full_name='JointDMPTrajectoryGeneratorMessage.num_basis', index=4, - number=5, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='num_sensor_values', full_name='JointDMPTrajectoryGeneratorMessage.num_sensor_values', index=5, - number=6, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='basis_mean', full_name='JointDMPTrajectoryGeneratorMessage.basis_mean', index=6, - number=7, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='basis_std', full_name='JointDMPTrajectoryGeneratorMessage.basis_std', index=7, - number=8, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='weights', full_name='JointDMPTrajectoryGeneratorMessage.weights', index=8, - number=9, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='initial_sensor_values', full_name='JointDMPTrajectoryGeneratorMessage.initial_sensor_values', index=9, - number=10, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=455, - serialized_end=684, -) - - -_POSEDMPTRAJECTORYGENERATORMESSAGE = _descriptor.Descriptor( - name='PoseDMPTrajectoryGeneratorMessage', - full_name='PoseDMPTrajectoryGeneratorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='orientation_only', full_name='PoseDMPTrajectoryGeneratorMessage.orientation_only', index=0, - number=1, type=8, cpp_type=7, label=2, - has_default_value=False, default_value=False, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='position_only', full_name='PoseDMPTrajectoryGeneratorMessage.position_only', index=1, - number=2, type=8, cpp_type=7, label=2, - has_default_value=False, default_value=False, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='ee_frame', full_name='PoseDMPTrajectoryGeneratorMessage.ee_frame', index=2, - number=3, type=8, cpp_type=7, label=2, - has_default_value=False, default_value=False, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='run_time', full_name='PoseDMPTrajectoryGeneratorMessage.run_time', index=3, - number=4, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='tau', full_name='PoseDMPTrajectoryGeneratorMessage.tau', index=4, - number=5, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='alpha', full_name='PoseDMPTrajectoryGeneratorMessage.alpha', index=5, - number=6, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='beta', full_name='PoseDMPTrajectoryGeneratorMessage.beta', index=6, - number=7, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='num_basis', full_name='PoseDMPTrajectoryGeneratorMessage.num_basis', index=7, - number=8, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='num_sensor_values', full_name='PoseDMPTrajectoryGeneratorMessage.num_sensor_values', index=8, - number=9, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='basis_mean', full_name='PoseDMPTrajectoryGeneratorMessage.basis_mean', index=9, - number=10, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='basis_std', full_name='PoseDMPTrajectoryGeneratorMessage.basis_std', index=10, - number=11, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='weights', full_name='PoseDMPTrajectoryGeneratorMessage.weights', index=11, - number=12, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='initial_sensor_values', full_name='PoseDMPTrajectoryGeneratorMessage.initial_sensor_values', index=12, - number=13, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=687, - serialized_end=982, -) - - -_QUATERNIONPOSEDMPTRAJECTORYGENERATORMESSAGE = _descriptor.Descriptor( - name='QuaternionPoseDMPTrajectoryGeneratorMessage', - full_name='QuaternionPoseDMPTrajectoryGeneratorMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='ee_frame', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.ee_frame', index=0, - number=1, type=8, cpp_type=7, label=2, - has_default_value=False, default_value=False, - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='run_time', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.run_time', index=1, - number=2, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='tau_pos', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.tau_pos', index=2, - number=3, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='alpha_pos', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.alpha_pos', index=3, - number=4, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='beta_pos', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.beta_pos', index=4, - number=5, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='tau_quat', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.tau_quat', index=5, - number=6, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='alpha_quat', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.alpha_quat', index=6, - number=7, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='beta_quat', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.beta_quat', index=7, - number=8, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='num_basis_pos', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.num_basis_pos', index=8, - number=9, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='num_sensor_values_pos', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.num_sensor_values_pos', index=9, - number=10, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='num_basis_quat', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.num_basis_quat', index=10, - number=11, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='num_sensor_values_quat', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.num_sensor_values_quat', index=11, - number=12, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='pos_basis_mean', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.pos_basis_mean', index=12, - number=13, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='pos_basis_std', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.pos_basis_std', index=13, - number=14, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='pos_weights', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.pos_weights', index=14, - number=15, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='pos_initial_sensor_values', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.pos_initial_sensor_values', index=15, - number=16, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='quat_basis_mean', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.quat_basis_mean', index=16, - number=17, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='quat_basis_std', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.quat_basis_std', index=17, - number=18, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='quat_weights', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.quat_weights', index=18, - number=19, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='quat_initial_sensor_values', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.quat_initial_sensor_values', index=19, - number=20, type=1, cpp_type=5, label=3, - has_default_value=False, default_value=[], - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='goal_time_quat', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.goal_time_quat', index=20, - number=21, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='goal_quat_w', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.goal_quat_w', index=21, - number=22, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='goal_quat_x', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.goal_quat_x', index=22, - number=23, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='goal_quat_y', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.goal_quat_y', index=23, - number=24, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - _descriptor.FieldDescriptor( - name='goal_quat_z', full_name='QuaternionPoseDMPTrajectoryGeneratorMessage.goal_quat_z', index=24, - number=25, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=985, - serialized_end=1605, -) - - -_RUNTIMEMESSAGE = _descriptor.Descriptor( - name='RunTimeMessage', - full_name='RunTimeMessage', - filename=None, - file=DESCRIPTOR, - containing_type=None, - fields=[ - _descriptor.FieldDescriptor( - name='run_time', full_name='RunTimeMessage.run_time', index=0, - number=1, type=1, cpp_type=5, label=2, - has_default_value=False, default_value=float(0), - message_type=None, enum_type=None, containing_type=None, - is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), - ], - extensions=[ - ], - nested_types=[], - enum_types=[ - ], - serialized_options=None, - is_extendable=False, - syntax='proto2', - extension_ranges=[], - oneofs=[ - ], - serialized_start=1607, - serialized_end=1641, -) - -DESCRIPTOR.message_types_by_name['GripperTrajectoryGeneratorMessage'] = _GRIPPERTRAJECTORYGENERATORMESSAGE -DESCRIPTOR.message_types_by_name['ImpulseTrajectoryGeneratorMessage'] = _IMPULSETRAJECTORYGENERATORMESSAGE -DESCRIPTOR.message_types_by_name['JointTrajectoryGeneratorMessage'] = _JOINTTRAJECTORYGENERATORMESSAGE -DESCRIPTOR.message_types_by_name['PoseTrajectoryGeneratorMessage'] = _POSETRAJECTORYGENERATORMESSAGE -DESCRIPTOR.message_types_by_name['JointDMPTrajectoryGeneratorMessage'] = _JOINTDMPTRAJECTORYGENERATORMESSAGE -DESCRIPTOR.message_types_by_name['PoseDMPTrajectoryGeneratorMessage'] = _POSEDMPTRAJECTORYGENERATORMESSAGE -DESCRIPTOR.message_types_by_name['QuaternionPoseDMPTrajectoryGeneratorMessage'] = _QUATERNIONPOSEDMPTRAJECTORYGENERATORMESSAGE -DESCRIPTOR.message_types_by_name['RunTimeMessage'] = _RUNTIMEMESSAGE -_sym_db.RegisterFileDescriptor(DESCRIPTOR) - -GripperTrajectoryGeneratorMessage = _reflection.GeneratedProtocolMessageType('GripperTrajectoryGeneratorMessage', (_message.Message,), { - 'DESCRIPTOR' : _GRIPPERTRAJECTORYGENERATORMESSAGE, - '__module__' : 'trajectory_generator_params_msg_pb2' - # @@protoc_insertion_point(class_scope:GripperTrajectoryGeneratorMessage) - }) -_sym_db.RegisterMessage(GripperTrajectoryGeneratorMessage) - -ImpulseTrajectoryGeneratorMessage = _reflection.GeneratedProtocolMessageType('ImpulseTrajectoryGeneratorMessage', (_message.Message,), { - 'DESCRIPTOR' : _IMPULSETRAJECTORYGENERATORMESSAGE, - '__module__' : 'trajectory_generator_params_msg_pb2' - # @@protoc_insertion_point(class_scope:ImpulseTrajectoryGeneratorMessage) - }) -_sym_db.RegisterMessage(ImpulseTrajectoryGeneratorMessage) - -JointTrajectoryGeneratorMessage = _reflection.GeneratedProtocolMessageType('JointTrajectoryGeneratorMessage', (_message.Message,), { - 'DESCRIPTOR' : _JOINTTRAJECTORYGENERATORMESSAGE, - '__module__' : 'trajectory_generator_params_msg_pb2' - # @@protoc_insertion_point(class_scope:JointTrajectoryGeneratorMessage) - }) -_sym_db.RegisterMessage(JointTrajectoryGeneratorMessage) - -PoseTrajectoryGeneratorMessage = _reflection.GeneratedProtocolMessageType('PoseTrajectoryGeneratorMessage', (_message.Message,), { - 'DESCRIPTOR' : _POSETRAJECTORYGENERATORMESSAGE, - '__module__' : 'trajectory_generator_params_msg_pb2' - # @@protoc_insertion_point(class_scope:PoseTrajectoryGeneratorMessage) - }) -_sym_db.RegisterMessage(PoseTrajectoryGeneratorMessage) - -JointDMPTrajectoryGeneratorMessage = _reflection.GeneratedProtocolMessageType('JointDMPTrajectoryGeneratorMessage', (_message.Message,), { - 'DESCRIPTOR' : _JOINTDMPTRAJECTORYGENERATORMESSAGE, - '__module__' : 'trajectory_generator_params_msg_pb2' - # @@protoc_insertion_point(class_scope:JointDMPTrajectoryGeneratorMessage) - }) -_sym_db.RegisterMessage(JointDMPTrajectoryGeneratorMessage) - -PoseDMPTrajectoryGeneratorMessage = _reflection.GeneratedProtocolMessageType('PoseDMPTrajectoryGeneratorMessage', (_message.Message,), { - 'DESCRIPTOR' : _POSEDMPTRAJECTORYGENERATORMESSAGE, - '__module__' : 'trajectory_generator_params_msg_pb2' - # @@protoc_insertion_point(class_scope:PoseDMPTrajectoryGeneratorMessage) - }) -_sym_db.RegisterMessage(PoseDMPTrajectoryGeneratorMessage) - -QuaternionPoseDMPTrajectoryGeneratorMessage = _reflection.GeneratedProtocolMessageType('QuaternionPoseDMPTrajectoryGeneratorMessage', (_message.Message,), { - 'DESCRIPTOR' : _QUATERNIONPOSEDMPTRAJECTORYGENERATORMESSAGE, - '__module__' : 'trajectory_generator_params_msg_pb2' - # @@protoc_insertion_point(class_scope:QuaternionPoseDMPTrajectoryGeneratorMessage) - }) -_sym_db.RegisterMessage(QuaternionPoseDMPTrajectoryGeneratorMessage) - -RunTimeMessage = _reflection.GeneratedProtocolMessageType('RunTimeMessage', (_message.Message,), { - 'DESCRIPTOR' : _RUNTIMEMESSAGE, - '__module__' : 'trajectory_generator_params_msg_pb2' - # @@protoc_insertion_point(class_scope:RunTimeMessage) - }) -_sym_db.RegisterMessage(RunTimeMessage) - - +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n%trajectory_generator_params_msg.proto\"_\n!GripperTrajectoryGeneratorMessage\x12\r\n\x05grasp\x18\x01 \x02(\x08\x12\r\n\x05width\x18\x02 \x02(\x01\x12\r\n\x05speed\x18\x03 \x02(\x01\x12\r\n\x05\x66orce\x18\x04 \x02(\x01\"\x8c\x01\n!ImpulseTrajectoryGeneratorMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01\x12\x10\n\x08\x61\x63\x63_time\x18\x02 \x02(\x01\x12\x11\n\tmax_trans\x18\x03 \x02(\x01\x12\x0f\n\x07max_rot\x18\x04 \x02(\x01\x12\x0e\n\x06\x66orces\x18\x05 \x03(\x01\x12\x0f\n\x07torques\x18\x06 \x03(\x01\"C\n\x1fJointTrajectoryGeneratorMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01\x12\x0e\n\x06joints\x18\x02 \x03(\x01\"r\n\'JointVelocityTrajectoryGeneratorMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01\x12\x18\n\x10joint_velocities\x18\x02 \x03(\x01\x12\x1b\n\x13joint_accelerations\x18\x03 \x03(\x01\"f\n\x1ePoseTrajectoryGeneratorMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01\x12\x10\n\x08position\x18\x02 \x03(\x01\x12\x12\n\nquaternion\x18\x03 \x03(\x01\x12\x0c\n\x04pose\x18\x04 \x03(\x01\"\xe5\x01\n\"JointDMPTrajectoryGeneratorMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01\x12\x0b\n\x03tau\x18\x02 \x02(\x01\x12\r\n\x05\x61lpha\x18\x03 \x02(\x01\x12\x0c\n\x04\x62\x65ta\x18\x04 \x02(\x01\x12\x11\n\tnum_basis\x18\x05 \x02(\x01\x12\x19\n\x11num_sensor_values\x18\x06 \x02(\x01\x12\x12\n\nbasis_mean\x18\x07 \x03(\x01\x12\x11\n\tbasis_std\x18\x08 \x03(\x01\x12\x0f\n\x07weights\x18\t \x03(\x01\x12\x1d\n\x15initial_sensor_values\x18\n \x03(\x01\"\xa7\x02\n!PoseDMPTrajectoryGeneratorMessage\x12\x18\n\x10orientation_only\x18\x01 \x02(\x08\x12\x15\n\rposition_only\x18\x02 \x02(\x08\x12\x10\n\x08\x65\x65_frame\x18\x03 \x02(\x08\x12\x10\n\x08run_time\x18\x04 \x02(\x01\x12\x0b\n\x03tau\x18\x05 \x02(\x01\x12\r\n\x05\x61lpha\x18\x06 \x02(\x01\x12\x0c\n\x04\x62\x65ta\x18\x07 \x02(\x01\x12\x11\n\tnum_basis\x18\x08 \x02(\x01\x12\x19\n\x11num_sensor_values\x18\t \x02(\x01\x12\x12\n\nbasis_mean\x18\n \x03(\x01\x12\x11\n\tbasis_std\x18\x0b \x03(\x01\x12\x0f\n\x07weights\x18\x0c \x03(\x01\x12\x1d\n\x15initial_sensor_values\x18\r \x03(\x01\"\xec\x04\n+QuaternionPoseDMPTrajectoryGeneratorMessage\x12\x10\n\x08\x65\x65_frame\x18\x01 \x02(\x08\x12\x10\n\x08run_time\x18\x02 \x02(\x01\x12\x0f\n\x07tau_pos\x18\x03 \x02(\x01\x12\x11\n\talpha_pos\x18\x04 \x02(\x01\x12\x10\n\x08\x62\x65ta_pos\x18\x05 \x02(\x01\x12\x10\n\x08tau_quat\x18\x06 \x02(\x01\x12\x12\n\nalpha_quat\x18\x07 \x02(\x01\x12\x11\n\tbeta_quat\x18\x08 \x02(\x01\x12\x15\n\rnum_basis_pos\x18\t \x02(\x01\x12\x1d\n\x15num_sensor_values_pos\x18\n \x02(\x01\x12\x16\n\x0enum_basis_quat\x18\x0b \x02(\x01\x12\x1e\n\x16num_sensor_values_quat\x18\x0c \x02(\x01\x12\x16\n\x0epos_basis_mean\x18\r \x03(\x01\x12\x15\n\rpos_basis_std\x18\x0e \x03(\x01\x12\x13\n\x0bpos_weights\x18\x0f \x03(\x01\x12!\n\x19pos_initial_sensor_values\x18\x10 \x03(\x01\x12\x17\n\x0fquat_basis_mean\x18\x11 \x03(\x01\x12\x16\n\x0equat_basis_std\x18\x12 \x03(\x01\x12\x14\n\x0cquat_weights\x18\x13 \x03(\x01\x12\"\n\x1aquat_initial_sensor_values\x18\x14 \x03(\x01\x12\x16\n\x0egoal_time_quat\x18\x15 \x02(\x01\x12\x13\n\x0bgoal_quat_w\x18\x16 \x02(\x01\x12\x13\n\x0bgoal_quat_x\x18\x17 \x02(\x01\x12\x13\n\x0bgoal_quat_y\x18\x18 \x02(\x01\x12\x13\n\x0bgoal_quat_z\x18\x19 \x02(\x01\"\"\n\x0eRunTimeMessage\x12\x10\n\x08run_time\x18\x01 \x02(\x01') + +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals()) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'trajectory_generator_params_msg_pb2', globals()) +if _descriptor._USE_C_DESCRIPTORS == False: + + DESCRIPTOR._options = None + _GRIPPERTRAJECTORYGENERATORMESSAGE._serialized_start=41 + _GRIPPERTRAJECTORYGENERATORMESSAGE._serialized_end=136 + _IMPULSETRAJECTORYGENERATORMESSAGE._serialized_start=139 + _IMPULSETRAJECTORYGENERATORMESSAGE._serialized_end=279 + _JOINTTRAJECTORYGENERATORMESSAGE._serialized_start=281 + _JOINTTRAJECTORYGENERATORMESSAGE._serialized_end=348 + _JOINTVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_start=350 + _JOINTVELOCITYTRAJECTORYGENERATORMESSAGE._serialized_end=464 + _POSETRAJECTORYGENERATORMESSAGE._serialized_start=466 + _POSETRAJECTORYGENERATORMESSAGE._serialized_end=568 + _JOINTDMPTRAJECTORYGENERATORMESSAGE._serialized_start=571 + _JOINTDMPTRAJECTORYGENERATORMESSAGE._serialized_end=800 + _POSEDMPTRAJECTORYGENERATORMESSAGE._serialized_start=803 + _POSEDMPTRAJECTORYGENERATORMESSAGE._serialized_end=1098 + _QUATERNIONPOSEDMPTRAJECTORYGENERATORMESSAGE._serialized_start=1101 + _QUATERNIONPOSEDMPTRAJECTORYGENERATORMESSAGE._serialized_end=1721 + _RUNTIMEMESSAGE._serialized_start=1723 + _RUNTIMEMESSAGE._serialized_end=1757 # @@protoc_insertion_point(module_scope) diff --git a/frankapy/proto_utils.py b/frankapy/proto_utils.py index 7091fad..8a917b2 100755 --- a/frankapy/proto_utils.py +++ b/frankapy/proto_utils.py @@ -1,6 +1,5 @@ from franka_interface_msgs.msg import SensorData, SensorDataGroup - def sensor_proto2ros_msg(sensor_proto_msg, sensor_data_type, info=''): sensor_ros_msg = SensorData() sensor_ros_msg.type = sensor_data_type @@ -8,22 +7,21 @@ def sensor_proto2ros_msg(sensor_proto_msg, sensor_data_type, info=''): sensor_data_bytes = sensor_proto_msg.SerializeToString() sensor_ros_msg.size = len(sensor_data_bytes) - sensor_ros_msg.sensorData = sensor_data_bytes + sensor_ros_msg.sensor_data = sensor_data_bytes return sensor_ros_msg - def make_sensor_group_msg(trajectory_generator_sensor_msg=None, feedback_controller_sensor_msg=None, termination_handler_sensor_msg=None): sensor_group_msg = SensorDataGroup() if trajectory_generator_sensor_msg is not None: sensor_group_msg.has_trajectory_generator_sensor_data = True - sensor_group_msg.trajectoryGeneratorSensorData = trajectory_generator_sensor_msg + sensor_group_msg.trajectory_generator_sensor_data = trajectory_generator_sensor_msg if feedback_controller_sensor_msg is not None: sensor_group_msg.has_feedback_controller_sensor_data = True - sensor_group_msg.feedbackControllerSensorData = feedback_controller_sensor_msg + sensor_group_msg.feedback_controller_sensor_data = feedback_controller_sensor_msg if termination_handler_sensor_msg is not None: sensor_group_msg.has_termination_handler_sensor_data = True - sensor_group_msg.terminationHandlerSensorData = termination_handler_sensor_msg + sensor_group_msg.termination_handler_sensor_data = termination_handler_sensor_msg return sensor_group_msg diff --git a/frankapy/ros_utils.py b/frankapy/ros_utils.py index ac8bcc8..81c672b 100644 --- a/frankapy/ros_utils.py +++ b/frankapy/ros_utils.py @@ -1,12 +1,14 @@ -import rospy +import rclpy +from rclpy.node import Node from visualization_msgs.msg import Marker, MarkerArray +from franka_interface_msgs.msg import SensorDataGroup import quaternion +class CollisionBoxesPublisher(Node): -class BoxesPublisher: - - def __init__(self, name, world_frame='panda_link0'): - self._boxes_pub = rospy.Publisher(name, MarkerArray, queue_size=10) + def __init__(self, topic_name, world_frame='panda_link0'): + super().__init__('collision_boxes_publisher') + self._boxes_pub = self.create_publisher(MarkerArray, topic_name, 10) self._world_frame = world_frame def publish_boxes(self, boxes): @@ -14,11 +16,11 @@ def publish_boxes(self, boxes): for i, box in enumerate(boxes): marker = Marker() marker.type = Marker.CUBE - marker.header.stamp = rospy.Time.now() + marker.header.stamp = self.get_clock().now() marker.header.frame_id = self._world_frame marker.id = i - marker.lifetime = rospy.Duration() + marker.lifetime = rclpy.time.Duration() marker.pose.position.x = box[0] marker.pose.position.y = box[1] diff --git a/frankapy/skill_list.py b/frankapy/skill_list.py index eecb0d3..6b8c560 100644 --- a/frankapy/skill_list.py +++ b/frankapy/skill_list.py @@ -1,7 +1,3 @@ -import roslib -roslib.load_manifest('franka_interface_msgs') -import rospy -import actionlib import numpy as np from autolab_core import RigidTransform @@ -10,7 +6,7 @@ from .proto import * from .utils import transform_to_list -from franka_interface_msgs.msg import ExecuteSkillAction, ExecuteSkillGoal +from franka_interface_msgs.action import ExecuteSkill class Skill: def __init__(self, @@ -33,9 +29,6 @@ def __init__(self, self._termination_handler_type = termination_handler_type self._timer_type = timer_type - self._sensor_value_sizes = 0 - self._initial_sensor_values = [] - # Add trajectory params self._trajectory_generator_param_data = [] self._trajectory_generator_param_data_size = 0 @@ -58,10 +51,6 @@ def set_meta_skill_type(self, meta_skill_type): def set_meta_skill_id(self, meta_skill_id): self._meta_skill_id = meta_skill_id - def add_initial_sensor_values(self, values): - self._initial_sensor_values = values - self._sensor_value_sizes = [len(values)] - def add_trajectory_params(self, params): self._trajectory_generator_param_data = params self._trajectory_generator_param_data_size = len(params) @@ -149,8 +138,8 @@ def add_internal_impedances(self, cartesian_impedances, joint_impedances): "Incorrect cartesian impedances len. Should be 0 or 6." assert len(joint_impedances) == 0 or len(joint_impedances) == 7, \ "Incorrect joint impedances len. Should be 0 or 7." - assert self._skill_type == SkillType.CartesianPoseSkill or self._skill_type == SkillType.JointPositionSkill, \ - "Incorrect skill type. Should be CartesianPoseSkill or JointPositionSkill." + assert self._skill_type == SkillType.CartesianPoseSkill or self._skill_type == SkillType.JointPositionSkill or self._skill_type == SkillType.JointVelocitySkill, \ + "Incorrect skill type. Should be CartesianPoseSkill, JointPositionSkill, or JointVelocitySkill." internal_feedback_controller_msg_proto = \ InternalImpedanceFeedbackControllerMessage( @@ -348,6 +337,20 @@ def add_goal_joints(self, run_time, joints): self.add_trajectory_params(joint_trajectory_generator_msg_proto.SerializeToString()) + def add_goal_joint_velocities(self, run_time, joint_velocities, joint_accelerations): + assert type(run_time) is float or type(run_time) is int,\ + "Incorrect time type. Should be int or float." + assert run_time >= 0, "Incorrect time. Should be non negative." + assert type(joint_velocities) is list, "Incorrect joint_velocities type. Should be list." + assert len(joint_velocities) == 7, "Incorrect joint_velocities len. Should be 7." + assert type(joint_accelerations) is list, "Incorrect joint_accelerations type. Should be list." + assert len(joint_accelerations) == 7, "Incorrect joint_accelerations len. Should be 7." + + joint_velocity_trajectory_generator_msg_proto = JointVelocityTrajectoryGeneratorMessage(run_time=run_time, + joint_velocities=joint_velocities, joint_accelerations=joint_accelerations) + + self.add_trajectory_params(joint_velocity_trajectory_generator_msg_proto.SerializeToString()) + def add_joint_dmp_params(self, run_time, joint_dmp_info, initial_sensor_values): assert type(run_time) is float or type(run_time) is int,\ "Incorrect run_time type. Should be int or float." @@ -523,14 +526,11 @@ def add_run_time(self, run_time): # Add checks for these def create_goal(self): - goal = ExecuteSkillGoal() + goal = ExecuteSkill.Goal() goal.skill_type = self._skill_type goal.skill_description = self._skill_desc goal.meta_skill_type = self._meta_skill_type goal.meta_skill_id = self._meta_skill_id - goal.sensor_topics = self._sensor_topics - goal.initial_sensor_values = self._initial_sensor_values - goal.sensor_value_sizes = self._sensor_value_sizes goal.trajectory_generator_type = self._trajectory_generator_type goal.trajectory_generator_param_data = self._trajectory_generator_param_data goal.trajectory_generator_param_data_size = self._trajectory_generator_param_data_size @@ -547,4 +547,4 @@ def create_goal(self): return goal def feedback_callback(self, feedback): - pass \ No newline at end of file + pass diff --git a/launch/franka_rviz.launch b/launch/franka_rviz.launch deleted file mode 100644 index a3927d3..0000000 --- a/launch/franka_rviz.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/ros2_ws/src/franka_interface_msgs b/ros2_ws/src/franka_interface_msgs new file mode 160000 index 0000000..13f66e1 --- /dev/null +++ b/ros2_ws/src/franka_interface_msgs @@ -0,0 +1 @@ +Subproject commit 13f66e16e8dbef617b6824f7b71a60873fce473e diff --git a/ros2_ws/src/franka_msgs/CMakeLists.txt b/ros2_ws/src/franka_msgs/CMakeLists.txt new file mode 100644 index 0000000..7e7a09d --- /dev/null +++ b/ros2_ws/src/franka_msgs/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.5) +project(franka_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} + "action/Grasp.action" + "action/Homing.action" + "action/Move.action" + "msg/GraspEpsilon.msg" + DEPENDENCIES std_msgs builtin_interfaces +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/ros2_ws/src/franka_msgs/action/Grasp.action b/ros2_ws/src/franka_msgs/action/Grasp.action new file mode 100644 index 0000000..d0b4bf5 --- /dev/null +++ b/ros2_ws/src/franka_msgs/action/Grasp.action @@ -0,0 +1,9 @@ +float64 width # [m] +GraspEpsilon epsilon +float64 speed # [m/s] +float64 force # [N] +--- +bool success +string error +--- +float64 current_width # [m] diff --git a/ros2_ws/src/franka_msgs/action/Homing.action b/ros2_ws/src/franka_msgs/action/Homing.action new file mode 100644 index 0000000..6ea0aa2 --- /dev/null +++ b/ros2_ws/src/franka_msgs/action/Homing.action @@ -0,0 +1,5 @@ +--- +bool success +string error +--- +float64 current_width # [m] diff --git a/ros2_ws/src/franka_msgs/action/Move.action b/ros2_ws/src/franka_msgs/action/Move.action new file mode 100644 index 0000000..34f37d6 --- /dev/null +++ b/ros2_ws/src/franka_msgs/action/Move.action @@ -0,0 +1,7 @@ +float64 width # [m] +float64 speed # [m/s] +--- +bool success +string error +--- +float64 current_width # [m] diff --git a/ros2_ws/src/franka_msgs/msg/GraspEpsilon.msg b/ros2_ws/src/franka_msgs/msg/GraspEpsilon.msg new file mode 100644 index 0000000..0efb391 --- /dev/null +++ b/ros2_ws/src/franka_msgs/msg/GraspEpsilon.msg @@ -0,0 +1,7 @@ +# Maximum tolerated deviation when the actual grasped width +# is smaller than the commanded grasp width. Unit: [m] +float64 inner 0.005 + +# Maximum tolerated deviation when the actual grasped width +# is larger than the commanded grasp width. Unit: [m] +float64 outer 0.005 \ No newline at end of file diff --git a/ros2_ws/src/franka_msgs/package.xml b/ros2_ws/src/franka_msgs/package.xml new file mode 100644 index 0000000..0b0c0db --- /dev/null +++ b/ros2_ws/src/franka_msgs/package.xml @@ -0,0 +1,26 @@ + + + + franka_msgs + 0.0.0 + franka_msgs provides messages and actions specific to Franka Emika research robots + Franka Emika GmbH + Apache 2.0 + + Franka Emika GmbH + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_default_generators + + builtin_interfaces + std_msgs + action_msgs + rosidl_default_runtime + + rosidl_interface_packages + + ament_cmake + + \ No newline at end of file diff --git a/rviz/franka_basic_config.rviz b/rviz/franka_basic_config.rviz deleted file mode 100644 index 4359365..0000000 --- a/rviz/franka_basic_config.rviz +++ /dev/null @@ -1,211 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Axes1 - Splitter Ratio: 0.5 - Tree Height: 775 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Class: rviz/Axes - Enabled: true - Length: 0.100000001 - Name: Axes - Radius: 0.00999999978 - Reference Frame: panda_end_effector - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: panda_link0 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.24045944 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.485398054 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.0903962478 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1855 - X: 65 - Y: 24 diff --git a/scripts/run_recorded_trajectory.py b/scripts/run_recorded_trajectory.py index 59f5938..6fdc8bf 100644 --- a/scripts/run_recorded_trajectory.py +++ b/scripts/run_recorded_trajectory.py @@ -3,18 +3,13 @@ import numpy as np import time -from autolab_core import RigidTransform from frankapy import FrankaArm, SensorDataMessageType -from frankapy import FrankaConstants as FC from frankapy.proto_utils import sensor_proto2ros_msg, make_sensor_group_msg from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage from franka_interface_msgs.msg import SensorDataGroup from frankapy.utils import convert_array_to_rigid_transform -import rospy - - if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument('--trajectory_pickle', '-t', type=str, required=True, @@ -26,7 +21,7 @@ fa = FrankaArm() fa.reset_joints() - rospy.loginfo('Loading Trajectory') + fa.log_info('Loading Trajectory') with open(args.trajectory_pickle, 'rb') as pkl_f: skill_data = pickle.load(pkl_f) @@ -46,11 +41,10 @@ duration=4.0, cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0]) - rospy.loginfo('Initializing Sensor Publisher') - pub = rospy.Publisher(FC.DEFAULT_SENSOR_PUBLISHER_TOPIC, SensorDataGroup, queue_size=10) - rate = rospy.Rate(1 / dt) + fa.log_info('Initializing Sensor Publisher') + rate = fa.get_rate(1 / dt) - rospy.loginfo('Publishing pose trajectory...') + fa.log_info('Publishing pose trajectory...') # To ensure skill doesn't end before completing trajectory, make the buffer time much longer than needed fa.goto_pose(convert_array_to_rigid_transform(pose_traj[1]), duration=T, @@ -58,9 +52,9 @@ buffer_time=10, cartesian_impedances=[600.0, 600.0, 600.0, 50.0, 50.0, 50.0] ) - init_time = rospy.Time.now().to_time() + init_time = fa.get_time().to_time() for i in range(2, len(ts)): - timestamp = rospy.Time.now().to_time() - init_time + timestamp = fa.get_time().to_time() - init_time pose_tf = convert_array_to_rigid_transform(pose_traj[i]) traj_gen_proto_msg = PosePositionSensorMessage( id=i, @@ -76,8 +70,8 @@ # Sleep the same amount as the trajectory was recorded in dt = skill_state_dict['time_since_skill_started'][i] - skill_state_dict['time_since_skill_started'][i-1] - rospy.loginfo('Publishing: ID {}, dt: {:.4f}'.format(traj_gen_proto_msg.id, dt)) - pub.publish(ros_msg) + fa.log_info('Publishing: ID {}, dt: {:.4f}'.format(traj_gen_proto_msg.id, dt)) + fa.publish_sensor_data(ros_msg) time.sleep(dt) # Finished trajectory if i >= pose_traj.shape[0] - 1: @@ -85,7 +79,7 @@ # Stop the skill # Alternatively can call fa.stop_skill() - term_proto_msg = ShouldTerminateSensorMessage(timestamp=rospy.Time.now().to_time() - init_time, + term_proto_msg = ShouldTerminateSensorMessage(timestamp=fa.get_time().to_time() - init_time, should_terminate=True) ros_msg = make_sensor_group_msg( termination_handler_sensor_msg=sensor_proto2ros_msg( @@ -93,4 +87,4 @@ ) pub.publish(ros_msg) - rospy.loginfo('Done') + fa.log_info('Done') diff --git a/setup.py b/setup.py index 7000f07..2a44ef2 100644 --- a/setup.py +++ b/setup.py @@ -6,16 +6,16 @@ requirements = [ 'autolab_core', 'empy', - 'numpy', + 'numpy==1.22.4', 'numpy-quaternion', 'numba', - 'rospkg', - 'catkin-tools', - 'protobuf==3.12.4' + 'catkin-pkg', + 'lark', + 'protobuf' ] setup(name='frankapy', - version='1.0.0', + version='2.0.0', description='FrankaPy Franka Panda Robot Control Library', author='Kevin Zhang, Mohit Sharma, Jacky Liang, Oliver Kroemer', author_email='', diff --git a/urdf/hand.urdf.xacro b/urdf/hand.urdf.xacro deleted file mode 100644 index 643fc60..0000000 --- a/urdf/hand.urdf.xacro +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/urdf/hand.xacro b/urdf/hand.xacro deleted file mode 100644 index 1cd3777..0000000 --- a/urdf/hand.xacro +++ /dev/null @@ -1,65 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf/panda_arm.urdf.xacro b/urdf/panda_arm.urdf.xacro deleted file mode 100644 index cfb3c23..0000000 --- a/urdf/panda_arm.urdf.xacro +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/urdf/panda_arm.xacro b/urdf/panda_arm.xacro deleted file mode 100644 index 672c881..0000000 --- a/urdf/panda_arm.xacro +++ /dev/null @@ -1,171 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/urdf/panda_arm_hand.urdf.xacro b/urdf/panda_arm_hand.urdf.xacro deleted file mode 100644 index 4c0f2fc..0000000 --- a/urdf/panda_arm_hand.urdf.xacro +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - -