diff --git a/CMakeLists.txt b/CMakeLists.txt index 0bae463..ff5d7d2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,8 @@ endif () find_library(CARTESIO_LIB CartesianInterface HINTS /opt/xbot/) if (CARTESIO_LIB) message(STATUS "Found CARTESIO_LIB ${CARTESIO_LIB}") + find_package(OpenSoT REQUIRED) + find_package(cartesian_interface REQUIRED) else () message(WARNING "CARTESIO_LIB not found") endif () @@ -97,6 +99,7 @@ add_service_files( FILES ExecuteAddCollisionBox.srv ExecuteAddCollisionPlane.srv + ExecuteAllLockedPoses.srv ExecuteAllPlans.srv ExecuteAllPoses.srv ExecuteAttachCollisionBox.srv @@ -166,8 +169,8 @@ generate_messages( # ) catkin_package( - # INCLUDE_DIRS include - LIBRARIES + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS ${ROS_DEPENDENCIES} ) @@ -223,14 +226,17 @@ if (HPP_CORE_LIB AND HPP_FCL_LIB) endif () if (CARTESIO_LIB) - include_directories(/opt/xbot/include) - add_library(${PROJECT_NAME}_CARTESIO_ADDON - src/lib/cartesio_server.cpp - ) - target_link_libraries(${PROJECT_NAME}_CARTESIO_ADDON + include_directories(/opt/xbot/include ${OpenSoT_INCLUDE_DIRS}) + add_library(${PROJECT_NAME}_cartesio src/lib/cartesio_server.cpp) + target_link_libraries(${PROJECT_NAME}_cartesio ${catkin_LIBRARIES}) + add_dependencies(${PROJECT_NAME}_cartesio ${PROJECT_NAME}_generate_messages_cpp) + + add_library(${PROJECT_NAME}_cartesio_addon src/lib/cartesio_addon.cpp) + add_dependencies(${PROJECT_NAME}_cartesio_addon ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + target_link_libraries(${PROJECT_NAME}_cartesio_addon ${catkin_LIBRARIES} - ) - add_dependencies(${PROJECT_NAME}_CARTESIO_ADDON ${PROJECT_NAME}_generate_messages_cpp) + ${OpenSoT_LIBRARIES} + ${cartesian_interface_LIBRARIES}) endif () ##################### @@ -288,7 +294,7 @@ if (CARTESIO_LIB) target_link_libraries(roport_cartesio_server ${catkin_LIBRARIES} ${PROJECT_NAME} - ${PROJECT_NAME}_CARTESIO_ADDON + ${PROJECT_NAME}_cartesio ) endif () @@ -301,12 +307,42 @@ catkin_install_python(PROGRAMS ${PY_SOURCES} DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +# Mark executables for installation +install(TARGETS + roport_task_scheduler + roport_robot_interface + roport_moveit_cpp_server + roport_msg_converter + roport_msg_aggregator + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +if (CARTESIO_LIB) + install(TARGETS roport_cartesio_server + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) +endif () + install(DIRECTORY launch tree DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) +# Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "common.h" + PATTERN ".svn" EXCLUDE + ) + +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + ############# ## Testing ## ############# diff --git a/README.md b/README.md index e575f22..2277845 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,10 @@ logo -[![CI](https://github.com/DrawZeroPoint/RoTools/actions/workflows/ci.yml/badge.svg?branch=master)](https://github.com/DrawZeroPoint/RoTools/actions/workflows/ci.yml) +

+CI +Code style: black +

RoTools is an all-in-one ROS package for high-level robotic task scheduling, visual perception, path planning, simulation, and direct-/tele-manipulation control. It leverages BehaviorTree to deliver fast task construction and diff --git a/include/roport/cartesio_server.h b/include/roport/cartesio_server.h index b08fc86..2204b4a 100644 --- a/include/roport/cartesio_server.h +++ b/include/roport/cartesio_server.h @@ -43,6 +43,7 @@ #include #include +#include #include #include @@ -64,16 +65,29 @@ class CartesIOServer { tf2_ros::TransformListener tf_listener_; ros::ServiceServer execute_all_poses_srv_; - ros::ServiceServer execute_mirrored_pose_srv_; + ros::ServiceServer execute_all_locked_poses_srv_; using reachPoseActionClient = actionlib::SimpleActionClient; std::vector> control_clients_; + /** + * Move the groups to corresponding poses. + * @param req + * @param resp + * @return + */ auto executeAllPosesSrvCb(roport::ExecuteAllPoses::Request& req, roport::ExecuteAllPoses::Response& resp) -> bool; - // auto executeMirroredPoseSrvCb(roport::ExecuteMirroredPose::Request& req, roport::ExecuteMirroredPose::Response& - // resp) - // -> bool; - // + + /** + * Move the groups to corresponding poses. If only one group is given, this is identical with executeAllPosesSrvCb, + * if multiple groups are given, the first group's pose will be the target, while other groups will move along with + * the first and keep their relative poses unchanged during the movement. + * @param req + * @param resp + * @return + */ + auto executeAllLockedPosesSrvCb(roport::ExecuteAllLockedPoses::Request& req, + roport::ExecuteAllLockedPoses::Response& resp) -> bool; void buildActionGoal(const int& index, const geometry_msgs::Pose& goal_pose, @@ -84,12 +98,23 @@ class CartesIOServer { auto executeGoals(const std::map& goals, double duration = 120) -> bool; - // static void getMirroredTrajectory(MoveGroupInterface& move_group, - // trajectory_msgs::JointTrajectory trajectory, - // std::vector mirror_vector, - // trajectory_msgs::JointTrajectory& mirrored_trajectory); - bool getTransform(const int& index, geometry_msgs::TransformStamped& transform); + + /** + * Given the index of the group in group_names_, get current pose of that group's control frame wrt the reference + * frame. + * @param index Group index in group_names_. + * @param pose Pose of the control frame. + * @return True if succeed, false otherwise. + */ + bool getCurrentPoseWithIndex(const int& index, geometry_msgs::Pose& pose); + + void getGoalPoseWithReference(const int& ref_idx, + const geometry_msgs::Pose& curr_ref_pose, + const geometry_msgs::Pose& goal_ref_pose, + const int& idx, + const geometry_msgs::Pose& curr_pose, + geometry_msgs::Pose& goal_pose); }; } // namespace roport diff --git a/include/roport/common.h b/include/roport/common.h index 64b26f7..fa52dc0 100644 --- a/include/roport/common.h +++ b/include/roport/common.h @@ -53,6 +53,14 @@ inline auto getParam(const ros::NodeHandle& node_handle, return true; } +inline auto getIndex(const std::vector& names, const std::string& target) -> int { + auto res = std::find(names.begin(), names.end(), target); + if (res != names.end()) { + return res - names.begin(); + } + return -1; +} + inline void geometryPoseToEigenMatrix(const geometry_msgs::Pose& pose, Eigen::Matrix4d& mat) { mat = Eigen::Matrix4d::Identity(); @@ -136,6 +144,24 @@ inline void localAlignedPoseToGlobalPose(const geometry_msgs::Pose& pose_local_a eigenMatrixToGeometryPose(mat_global_to_target, pose_global_to_target); } +inline void toGlobalPose(const int& goal_type, + const geometry_msgs::Pose& current_pose, + const geometry_msgs::Pose& cmd_pose, + geometry_msgs::Pose& goal_pose) { + if (goal_type == 0) { + // The given pose is already in the reference frame + goal_pose = cmd_pose; + } else if (goal_type == 1) { + // The given pose is relative to the local aligned frame having the same orientation as the reference frame + localAlignedPoseToGlobalPose(cmd_pose, current_pose, goal_pose, true); + } else if (goal_type == 2) { + // The given pose is relative to the local frame + localPoseToGlobalPose(cmd_pose, current_pose, goal_pose); + } else { + throw std::invalid_argument("Goal type not supported"); + } +} + inline auto isPoseLegal(const geometry_msgs::Pose& pose) -> bool { if (pose.orientation.w == 0 && pose.orientation.x == 0 && pose.orientation.y == 0 && pose.orientation.z == 0) { ROS_ERROR("The pose is empty (all orientation coefficients are zero)"); @@ -149,6 +175,60 @@ inline auto isPoseLegal(const geometry_msgs::Pose& pose) -> bool { return true; } +/** + * Converting a quaternion representing a planar rotation to theta. + * @param quat Quaternion msg. + * @return theta. + */ +inline double quaternionToTheta(const geometry_msgs::Quaternion& quat) { + Eigen::Quaterniond quaternion(quat.w, quat.x, quat.y, quat.z); + Eigen::Rotation2Dd rot(quaternion.toRotationMatrix().topLeftCorner<2, 2>()); + return rot.smallestPositiveAngle(); +} + +/** + * Pretty print a Eigen matrix. Its size, row number, and column number will also be displayed. + * @param mat A matrix with various shape to print. + * @param precision How many digits to keep after the decimal point. + */ +inline void prettyPrintEigenMatrix(const Eigen::Matrix& mat, + const int& precision = 3) { + Eigen::IOFormat cleanFormat(precision, 0, ", ", "\n", "[", "]"); + std::cout << "Matrix size: " << mat.size() << " rows: " << mat.rows() << " cols: " << mat.cols() << std::endl; + std::cout << mat.format(cleanFormat) << std::endl; +} + +inline void getCartesianError(const Eigen::Vector3d& current_position, + const Eigen::Quaterniond& current_orientation, + const Eigen::Vector3d& goal_position, + const Eigen::Quaterniond& goal_orientation, + Eigen::Matrix& error) { + error.head(3) << current_position - goal_position; + + Eigen::Affine3d current_affine_transform; + current_affine_transform.translation() = current_position; + current_affine_transform.linear() = current_orientation.toRotationMatrix(); + + Eigen::Quaterniond regularized_current_orientation; + regularized_current_orientation.coeffs() << current_orientation.coeffs(); + if (goal_orientation.coeffs().dot(current_orientation.coeffs()) < 0.) { + regularized_current_orientation.coeffs() << -current_orientation.coeffs(); + } + Eigen::Quaterniond error_quaternion(regularized_current_orientation.inverse() * goal_orientation); + error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z(); + error.tail(3) << -current_affine_transform.linear() * error.tail(3); +} + +inline void getCartesianError(const Eigen::Affine3d& current_transform, + const Eigen::Affine3d& goal_transform, + Eigen::Matrix& error) { + Eigen::Vector3d current_position(current_transform.translation()); + Eigen::Quaterniond current_orientation(current_transform.linear()); + Eigen::Vector3d goal_position(goal_transform.translation()); + Eigen::Quaterniond goal_orientation(goal_transform.linear()); + getCartesianError(current_position, current_orientation, goal_position, goal_orientation, error); +} + /** * Judge if all corresponding elements in the two given vectors are close to each other under the tolerance. * @tparam T Value type. diff --git a/scripts/roport_flask_server_template.py b/scripts/roport_flask_server_template.py index d6f8031..6c5215f 100644 --- a/scripts/roport_flask_server_template.py +++ b/scripts/roport_flask_server_template.py @@ -17,11 +17,11 @@ # Custom functions # TODO add this -''' +""" Note that this is a template for guiding the usage of Flask server for porting the algorithm using Python3. You could implement the server for your algorithm by referring this. -''' +""" app = Flask(__name__, template_folder="templates") @@ -49,14 +49,18 @@ def decode_b64_to_image(b64_str, is_bgr=True): img = base64.b64decode(b64_str) # imdecode use the same flag as imread. cf. https://docs.opencv.org/3.4/d8/d6a/group__imgcodecs__flags.html if is_bgr: - return True, cv2.imdecode(np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_COLOR) + return True, cv2.imdecode( + np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_COLOR + ) else: - return True, cv2.imdecode(np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_ANYDEPTH) + return True, cv2.imdecode( + np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_ANYDEPTH + ) except cv2.error: return False, None -@app.route('/process', methods=['POST']) +@app.route("/process", methods=["POST"]) def process(): """The main entrance of the server. @@ -65,22 +69,22 @@ def process(): """ try: req_data = json.loads(request.data) - if 'body' not in req_data: + if "body" not in req_data: return make_response("Failed to load request data", 200) else: - req_body = req_data['body'] + req_body = req_data["body"] except ValueError: return make_response("Failed to phase JSON", 200) header = {} - response = {'status': False, 'results': []} + response = {"status": False, "results": []} cv_image_list = [] - image_strings = json.loads(req_body['images']) + image_strings = json.loads(req_body["images"]) for s in image_strings: ok, image = decode_b64_to_image(s) if not ok: - feedback = {'header': header, 'response': response} + feedback = {"header": header, "response": response} return make_response(json.dumps(feedback), 200) cv_image_list.append(image) @@ -88,11 +92,11 @@ def process(): torch.cuda.empty_cache() if ok: - response['status'] = True - response['results'] = json.dumps(results) - feedback = {'header': header, 'response': response} + response["status"] = True + response["results"] = json.dumps(results) + feedback = {"header": header, "response": response} return make_response(json.dumps(feedback), 200) if __name__ == "__main__": - waitress.serve(app, host='0.0.0.0', port=6060, threads=6) + waitress.serve(app, host="0.0.0.0", port=6060, threads=6) diff --git a/scripts/roport_hpp_manipulation_client.py b/scripts/roport_hpp_manipulation_client.py index 1be2696..5539c25 100755 --- a/scripts/roport_hpp_manipulation_client.py +++ b/scripts/roport_hpp_manipulation_client.py @@ -9,51 +9,58 @@ from rotools.utility.common import get_param, pretty_print_configs -if __name__ == '__main__': +if __name__ == "__main__": server_process = None if sys.version_info >= (3, 2): server_process = subprocess.Popen(["hppcorbaserver"], start_new_session=True) else: - rospy.logwarn("You need to manually run 'hppcorbaserver' before launching this program") + rospy.logwarn( + "You need to manually run 'hppcorbaserver' before launching this program" + ) try: - rospy.init_node('roport_hpp_manipulation_client') + rospy.init_node("roport_hpp_manipulation_client") configs = { - 'env_name': get_param('~env_name'), - 'env_pkg_name': get_param('~env_pkg_name'), - 'env_surface': get_param('~env_surface'), - 'object_name': get_param('~object_name'), - 'object_pkg_name': get_param('~object_pkg_name'), - 'object_surface': get_param('~object_surface'), - 'object_handle': get_param('~object_handle'), - 'robot_name': get_param('~robot_name'), - 'robot_pkg_name': get_param('~robot_pkg_name'), - 'robot_urdf_name': get_param('~robot_urdf_name'), - 'robot_srdf_name': get_param('~robot_srdf_name'), - 'robot_bound': get_param('~robot_bound'), - 'object_bound': get_param('~object_bound'), - 'gripper_name': get_param('~gripper_name'), - 'fingers': get_param('~fingers'), - 'finger_joints': get_param('~finger_joints'), - 'finger_joint_values': get_param('~finger_joint_values'), - 'joint_state_topic': get_param('~joint_state_topic'), - 'odom_topic': get_param('~odom_topic'), - 'object_pose_topic': get_param('~object_pose_topic'), - 'joint_cmd_topic': get_param('~joint_cmd_topic'), - 'base_cmd_topic': get_param('~base_cmd_topic'), - 'enable_viewer': get_param('~enable_viewer'), - 'reduction_ratio': get_param('~reduction_ratio', 0.2), + "env_name": get_param("~env_name"), + "env_pkg_name": get_param("~env_pkg_name"), + "env_surface": get_param("~env_surface"), + "object_name": get_param("~object_name"), + "object_pkg_name": get_param("~object_pkg_name"), + "object_surface": get_param("~object_surface"), + "object_handle": get_param("~object_handle"), + "robot_name": get_param("~robot_name"), + "robot_pkg_name": get_param("~robot_pkg_name"), + "robot_urdf_name": get_param("~robot_urdf_name"), + "robot_srdf_name": get_param("~robot_srdf_name"), + "robot_bound": get_param("~robot_bound"), + "object_bound": get_param("~object_bound"), + "gripper_name": get_param("~gripper_name"), + "fingers": get_param("~fingers"), + "finger_joints": get_param("~finger_joints"), + "finger_joint_values": get_param("~finger_joint_values"), + "joint_state_topic": get_param("~joint_state_topic"), + "odom_topic": get_param("~odom_topic"), + "object_pose_topic": get_param("~object_pose_topic"), + "joint_cmd_topic": get_param("~joint_cmd_topic"), + "base_cmd_topic": get_param("~base_cmd_topic"), + "enable_viewer": get_param("~enable_viewer"), + "reduction_ratio": get_param("~reduction_ratio", 0.2), } viewer_process = None - if configs['enable_viewer']: + if configs["enable_viewer"]: if sys.version_info >= (3, 2): - viewer_process = subprocess.Popen(["gepetto-gui", "-c", "basic"], - stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, - start_new_session=True) + viewer_process = subprocess.Popen( + ["gepetto-gui", "-c", "basic"], + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + start_new_session=True, + ) else: - rospy.logwarn("You need to manually run 'gepetto-gui -c basic' before launching this program") + rospy.logwarn( + "You need to manually run 'gepetto-gui -c basic' before launching this program" + ) pretty_print_configs(configs) client = HPPManipulationClient(configs) diff --git a/scripts/roport_moveit_py_server.py b/scripts/roport_moveit_py_server.py index 3d0330c..02bd510 100755 --- a/scripts/roport_moveit_py_server.py +++ b/scripts/roport_moveit_py_server.py @@ -9,19 +9,19 @@ if __name__ == "__main__": try: - rospy.init_node('roport_moveit_py_server') + rospy.init_node("roport_moveit_py_server") # You only need to modify the config to apply this to new robots - group_names = get_param('~group_names') + group_names = get_param("~group_names") assert group_names is not None - ee_links = get_param('~ee_links') + ee_links = get_param("~ee_links") # Will be the base frame of the planning group if not given - ref_frames = get_param('~ref_frames') + ref_frames = get_param("~ref_frames") configs = { - 'robot_description': 'robot_description', - 'ns': '', - 'group_names': group_names, - 'ee_links': ee_links, - 'ref_frames': ref_frames, + "robot_description": "robot_description", + "ns": "", + "group_names": group_names, + "ee_links": ee_links, + "ref_frames": ref_frames, } pretty_print_configs(configs) server = MoveItServer(configs) diff --git a/scripts/roport_mujoco_server.py b/scripts/roport_mujoco_server.py index 2d7d8ba..fba42e9 100755 --- a/scripts/roport_mujoco_server.py +++ b/scripts/roport_mujoco_server.py @@ -8,16 +8,16 @@ if __name__ == "__main__": try: - rospy.init_node('roport_mujoco_server') + rospy.init_node("roport_mujoco_server") - model_path = '.mujoco/mujoco214/model/humanoid/humanoid.xml' + model_path = ".mujoco/mujoco214/model/humanoid/humanoid.xml" configs = { - 'model_path': get_path('model_path', model_path), - 'kinematics_path': '', - 'actuator_path': '', - 'enable_viewer': get_param('enable_viewer', True), - 'publish_rate': get_param('publish_rate', 60.), + "model_path": get_path("model_path", model_path), + "kinematics_path": "", + "actuator_path": "", + "enable_viewer": get_param("enable_viewer", True), + "publish_rate": get_param("publish_rate", 60.0), } pretty_print_configs(configs) diff --git a/scripts/roport_optitrack_client.py b/scripts/roport_optitrack_client.py index d482734..a429a43 100755 --- a/scripts/roport_optitrack_client.py +++ b/scripts/roport_optitrack_client.py @@ -4,23 +4,28 @@ from rotools.optitrack.linux.client import OptiTrackClient -from rotools.utility.common import get_param, pretty_print_configs, is_ip_valid, is_port_valid +from rotools.utility.common import ( + get_param, + pretty_print_configs, + is_ip_valid, + is_port_valid, +) -if __name__ == '__main__': +if __name__ == "__main__": try: - rospy.init_node('roport_optitrack_client', anonymous=True) + rospy.init_node("roport_optitrack_client", anonymous=True) configs = { - 'ip': get_param('ip'), - 'port': get_param('port'), - 'odom_topic': get_param('odom_topic'), - 'pose_topic': get_param('pose_topic'), - 'rate': get_param('rate', 100.), - 'transform': get_param('base_to_markers_transform'), + "ip": get_param("ip"), + "port": get_param("port"), + "odom_topic": get_param("odom_topic"), + "pose_topic": get_param("pose_topic"), + "rate": get_param("rate", 100.0), + "transform": get_param("base_to_markers_transform"), } - if not is_ip_valid(configs['ip']) or not is_port_valid(configs['port']): + if not is_ip_valid(configs["ip"]) or not is_port_valid(configs["port"]): exit(-1) pretty_print_configs(configs) diff --git a/scripts/roport_planner_server.py b/scripts/roport_planner_server.py index 866bc99..6af6c12 100755 --- a/scripts/roport_planner_server.py +++ b/scripts/roport_planner_server.py @@ -9,14 +9,14 @@ if __name__ == "__main__": try: - rospy.init_node('roport_planner_server') + rospy.init_node("roport_planner_server") configs = { - 'mode': get_param('~mode'), - 'control_topics': get_param('~control_topics'), - 'rate': get_param('~rate'), - 'algorithm_port': get_param('~algorithm_port'), - 'robot_initial_poses': get_param('~robot_initial_poses'), - 'planning_initial_poses': get_param('~planning_initial_poses'), + "mode": get_param("~mode"), + "control_topics": get_param("~control_topics"), + "rate": get_param("~rate"), + "algorithm_port": get_param("~algorithm_port"), + "robot_initial_poses": get_param("~robot_initial_poses"), + "planning_initial_poses": get_param("~planning_initial_poses"), } pretty_print_configs(configs) diff --git a/scripts/roport_recorder.py b/scripts/roport_recorder.py index 99a32f1..9aedb51 100755 --- a/scripts/roport_recorder.py +++ b/scripts/roport_recorder.py @@ -16,12 +16,12 @@ def __init__(self): super(Recorder, self).__init__() # The topic publishing joint states (should be only one topic) - js_topic = get_param('~js_topic') + js_topic = get_param("~js_topic") # The joint names needed to be recorded (optional, if not provided, record all joints in js_topic) - self.joint_names = get_param('~joint_names') + self.joint_names = get_param("~joint_names") # The topic publishing image TODO record multiple images - img_topic = get_param('~img_topic') + img_topic = get_param("~img_topic") self.write_position = False self.write_image = False @@ -29,10 +29,12 @@ def __init__(self): self.max_positions = None self.min_positions = None self.images = [] - self.save_dir = '/home/dzp/' + self.save_dir = "/home/dzp/" self.js_sub = rospy.Subscriber(js_topic, JointState, self.js_cb, queue_size=1) self.img_sub = rospy.Subscriber(img_topic, Image, self.img_cb, queue_size=1) - self.cmd_sub = rospy.Subscriber('/recorder/cmd', String, self.cmd_cb, queue_size=1) + self.cmd_sub = rospy.Subscriber( + "/recorder/cmd", String, self.cmd_cb, queue_size=1 + ) def js_cb(self, msg): if not self.write_position: @@ -43,14 +45,14 @@ def js_cb(self, msg): positions.append(msg.position[i]) positions = np.asarray(positions) if self.max_positions is None: - self.max_positions = np.ones_like(positions) * -100. + self.max_positions = np.ones_like(positions) * -100.0 self.max_positions = np.amax((self.max_positions, positions), axis=0) if self.min_positions is None: - self.min_positions = np.ones_like(positions) * 100. + self.min_positions = np.ones_like(positions) * 100.0 self.min_positions = np.amin((self.min_positions, positions), axis=0) - print('Added new position (deg): \n', positions * 180 / np.pi) - print('Max position (rad): \n', self.max_positions) - print('Min position (rad): \n', self.min_positions) + print("Added new position (deg): \n", positions * 180 / np.pi) + print("Max position (rad): \n", self.max_positions) + print("Min position (rad): \n", self.min_positions) self.positions.append(positions) self.write_position = False @@ -58,21 +60,23 @@ def img_cb(self, msg): if not self.write_image: return bridge = CvBridge() - cv_image = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') + cv_image = bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") self.images.append(cv_image) - cv2.imwrite(os.path.join(self.save_dir, '{}.jpg'.format(len(self.images))), cv_image) - print('Added new image: \n', cv_image.shape) + cv2.imwrite( + os.path.join(self.save_dir, "{}.jpg".format(len(self.images))), cv_image + ) + print("Added new image: \n", cv_image.shape) self.write_image = False def cmd_cb(self, msg): - if 'j' in msg.data: - rospy.loginfo('recording joint states...') + if "j" in msg.data: + rospy.loginfo("recording joint states...") self.record_js() - if 'i' in msg.data: - rospy.loginfo('recording image...') + if "i" in msg.data: + rospy.loginfo("recording image...") self.record_img() - if msg.data == 's': - rospy.loginfo('saving...') + if msg.data == "s": + rospy.loginfo("saving...") self.save_js() def record_js(self): @@ -84,18 +88,20 @@ def record_img(self): def save_js(self): p = np.asarray(self.positions) np.save(os.path.join(self.save_dir, "samples_js.npy"), p) - rospy.loginfo('Saved {} joint states'.format(len(p))) + rospy.loginfo("Saved {} joint states".format(len(p))) i = np.asarray(self.images) np.save(os.path.join(self.save_dir, "samples_img.npy"), i) - rospy.loginfo('Saved {} images'.format(len(i))) + rospy.loginfo("Saved {} images".format(len(i))) if __name__ == "__main__": try: - rospy.init_node('roport_recorder') + rospy.init_node("roport_recorder") recorder = Recorder() rospy.loginfo("RoPort: Recorder ready.") - rospy.loginfo('Send String msg to /recorder/cmd (j: record joint states, i: record image, s: save)') + rospy.loginfo( + "Send String msg to /recorder/cmd (j: record joint states, i: record image, s: save)" + ) rospy.spin() except rospy.ROSInterruptException as e: print(e) diff --git a/scripts/roport_rollout_server.py b/scripts/roport_rollout_server.py index f7ff959..e8b66cd 100755 --- a/scripts/roport_rollout_server.py +++ b/scripts/roport_rollout_server.py @@ -11,13 +11,15 @@ from rotools.utility import common -root_dir = '/home/smart/data' -print('Project root dir {}'.format(root_dir)) +root_dir = "/home/smart/data" +print("Project root dir {}".format(root_dir)) -poses_pub = rospy.Publisher('/rollout', PoseArray, queue_size=1) -left_pub = rospy.Publisher('/cartesian/left_hand/reference', PoseStamped, queue_size=1) -right_pub = rospy.Publisher('/cartesian/right_hand/reference', PoseStamped, queue_size=1) +poses_pub = rospy.Publisher("/rollout", PoseArray, queue_size=1) +left_pub = rospy.Publisher("/cartesian/left_hand/reference", PoseStamped, queue_size=1) +right_pub = rospy.Publisher( + "/cartesian/right_hand/reference", PoseStamped, queue_size=1 +) def publish_rollout(rollout, rate): @@ -26,9 +28,9 @@ def publish_rollout(rollout, rate): pose_array = PoseArray() left_pose_stamped = PoseStamped() right_pose_stamped = PoseStamped() - pose_array.header.frame_id = 'map' - left_pose_stamped.header.frame_id = 'ci/torso' - right_pose_stamped.header.frame_id = 'ci/torso' + pose_array.header.frame_id = "map" + left_pose_stamped.header.frame_id = "ci/torso" + right_pose_stamped.header.frame_id = "ci/torso" left_p_quat = unit[0, :] right_p_quat = unit[1, :] left_pose = common.to_ros_pose(left_p_quat) @@ -41,28 +43,28 @@ def publish_rollout(rollout, rate): left_pub.publish(left_pose_stamped) right_pub.publish(right_pose_stamped) - rospy.loginfo('Step: {}/{}'.format(i, n_rollout)) + rospy.loginfo("Step: {}/{}".format(i, n_rollout)) rospy.sleep(rospy.Duration.from_sec(1.0 / rate)) @click.command() -@click.option('-n', '--name', default='ctrl_rollout') -@click.option('-r', '--rate', default=10.0) +@click.option("-n", "--name", default="ctrl_rollout") +@click.option("-r", "--rate", default=10.0) def run(name, rate): - rollout_path = os.path.join(root_dir, name + '.npy') + rollout_path = os.path.join(root_dir, name + ".npy") if not os.path.exists(rollout_path): - rospy.logerr('Rollout path {} is empty'.format(rollout_path)) + rospy.logerr("Rollout path {} is empty".format(rollout_path)) return rollout = np.load(rollout_path) if rollout.shape[-1] == 0: - rospy.logwarn('Rollout {} is empty'.format(rollout_path)) + rospy.logwarn("Rollout {} is empty".format(rollout_path)) else: publish_rollout(rollout, rate) if __name__ == "__main__": try: - rospy.init_node('rollout_visualization') + rospy.init_node("rollout_visualization") run() rospy.spin() except rospy.ROSInterruptException as e: diff --git a/scripts/roport_sensing_server.py b/scripts/roport_sensing_server.py index eaafa31..1158f44 100755 --- a/scripts/roport_sensing_server.py +++ b/scripts/roport_sensing_server.py @@ -9,20 +9,22 @@ if __name__ == "__main__": try: - rospy.init_node('roport_sensing_server') + rospy.init_node("roport_sensing_server") # You only need to modify the config to apply this to new robots - device_names = get_param('~device_names') + device_names = get_param("~device_names") assert device_names is not None, print("No sensing device available") - algorithm_names = get_param('~algorithm_names') - algorithm_ports = get_param('~algorithm_ports') + algorithm_names = get_param("~algorithm_names") + algorithm_ports = get_param("~algorithm_ports") assert algorithm_ports is not None, print("No sensing algorithm available") if algorithm_names and algorithm_ports: - assert len(algorithm_names) == len(algorithm_ports), print('Algorithm names mismatch ports') + assert len(algorithm_names) == len(algorithm_ports), print( + "Algorithm names mismatch ports" + ) configs = { - 'device_names': device_names, - 'algorithm_names': algorithm_names, - 'algorithm_ports': algorithm_ports, + "device_names": device_names, + "algorithm_names": algorithm_names, + "algorithm_ports": algorithm_ports, } pretty_print_configs(configs) server = SensingServer(configs) diff --git a/scripts/roport_snapshot_server.py b/scripts/roport_snapshot_server.py index f7297b9..5297229 100755 --- a/scripts/roport_snapshot_server.py +++ b/scripts/roport_snapshot_server.py @@ -7,15 +7,15 @@ if __name__ == "__main__": try: - rospy.init_node('roport_snapshot_server') + rospy.init_node("roport_snapshot_server") configs = { - 'js_topics': get_param('~js_topics'), - 'odom_topics': get_param('~odom_topics'), - 'pose_topics': get_param('~pose_topics'), - 'rgb_compressed_topics': get_param('~rgb_compressed_topics'), - 'depth_compressed_topics': get_param('~depth_compressed_topics'), - 'save_dir': get_param('~save_dir'), + "js_topics": get_param("~js_topics"), + "odom_topics": get_param("~odom_topics"), + "pose_topics": get_param("~pose_topics"), + "rgb_compressed_topics": get_param("~rgb_compressed_topics"), + "depth_compressed_topics": get_param("~depth_compressed_topics"), + "save_dir": get_param("~save_dir"), } pretty_print_configs(configs) diff --git a/scripts/roport_websocket_client.py b/scripts/roport_websocket_client.py index 8daf114..942bab7 100755 --- a/scripts/roport_websocket_client.py +++ b/scripts/roport_websocket_client.py @@ -4,21 +4,26 @@ from rotools.websocket.core.client import WebsocketClient -from rotools.utility.common import get_param, pretty_print_configs, is_ip_valid, is_port_valid +from rotools.utility.common import ( + get_param, + pretty_print_configs, + is_ip_valid, + is_port_valid, +) -if __name__ == '__main__': +if __name__ == "__main__": try: - rospy.init_node('roport_websocket_client', anonymous=True) + rospy.init_node("roport_websocket_client", anonymous=True) configs = { - 'ip': get_param('~ip'), - 'port': get_param('~port'), - 'from_client_topics': get_param('~from_client_topics'), - 'to_client_topics': get_param('~to_client_topics'), + "ip": get_param("~ip"), + "port": get_param("~port"), + "from_client_topics": get_param("~from_client_topics"), + "to_client_topics": get_param("~to_client_topics"), } - if not is_ip_valid(configs['ip']) or not is_port_valid(configs['port']): + if not is_ip_valid(configs["ip"]) or not is_port_valid(configs["port"]): exit(-1) pretty_print_configs(configs) diff --git a/scripts/roport_xsens_server.py b/scripts/roport_xsens_server.py index b5eb1b9..7b35b6c 100755 --- a/scripts/roport_xsens_server.py +++ b/scripts/roport_xsens_server.py @@ -4,30 +4,36 @@ import rospy from rotools.xsens.core.server import XsensServer -from rotools.utility.common import get_param, pretty_print_configs, is_ip_valid, is_port_valid +from rotools.utility.common import ( + get_param, + pretty_print_configs, + is_ip_valid, + is_port_valid, +) if __name__ == "__main__": try: - rospy.init_node('roport_xsens_server') + rospy.init_node("roport_xsens_server") configs = { - 'udp_ip': get_param('~ip', ''), - 'udp_port': get_param('~port', 9763), - 'ref_frame': get_param('~ref_frame', 'world'), - 'scaling': get_param('~scaling', 1.0), - 'rate': get_param('~rate', 60.), - 'prop': get_param('~prop', False) + "udp_ip": get_param("~ip", ""), + "udp_port": get_param("~port", 9763), + "ref_frame": get_param("~ref_frame", "world"), + "scaling": get_param("~scaling", 1.0), + "rate": get_param("~rate", 60.0), + "prop": get_param("~prop", False), } # If udp_ip is not given, get local IP as UDP IP - if configs['udp_ip'] == '': + if configs["udp_ip"] == "": import socket + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) s.connect(("8.8.8.8", 80)) - configs['udp_ip'] = s.getsockname()[0] + configs["udp_ip"] = s.getsockname()[0] s.close() - if not is_ip_valid(configs['udp_ip']) or not is_port_valid(configs['port']): + if not is_ip_valid(configs["udp_ip"]) or not is_port_valid(configs["port"]): exit(-1) pretty_print_configs(configs) diff --git a/src/lib/cartesio_addon.cpp b/src/lib/cartesio_addon.cpp new file mode 100644 index 0000000..87d76b4 --- /dev/null +++ b/src/lib/cartesio_addon.cpp @@ -0,0 +1,129 @@ +#include +#include +#include +#include +#include +#include + +using namespace XBot::Cartesian; + +struct CentauroSteeringTask : public TaskDescriptionImpl { + CARTESIO_DECLARE_SMART_PTR(CentauroSteeringTask) + + std::string wheel_name; + double max_steering_speed; + Eigen::Vector3d contact_plane_normal; + + CentauroSteeringTask(YAML::Node task_node, Context::ConstPtr ctx); +}; + +CentauroSteeringTask::CentauroSteeringTask(YAML::Node task_node, Context::ConstPtr ctx) + : TaskDescriptionImpl(task_node, std::move(ctx), "steering_" + task_node["wheel_name"].as(), 1), + contact_plane_normal(0, 0, 1.) { + wheel_name = task_node["wheel_name"].as(); + + if (task_node["max_steering_speed"]) { + max_steering_speed = task_node["max_steering_speed"].as(); + } else { + max_steering_speed = 3.0; + } + + if (task_node["contact_plane_normal"]) { + auto contact_plane_normal_std = task_node["contact_plane_normal"].as>(); + + if (contact_plane_normal_std.size() != 3) { + throw std::runtime_error("contact_plane_normal size() must be 3"); + } + + contact_plane_normal = Eigen::Vector3d::Map(contact_plane_normal_std.data()); + } +} + +CARTESIO_REGISTER_TASK_PLUGIN(CentauroSteeringTask, CentauroSteering); + +class CentauroSteeringOpenSot : public OpenSotTaskAdapter { + public: + CentauroSteeringOpenSot(const TaskDescription::Ptr& task_desc, Context::ConstPtr ctx) + : OpenSotTaskAdapter(task_desc, std::move(ctx)) { + ci_steering_ = std::dynamic_pointer_cast(task_desc); + + if (!ci_steering_) + throw std::runtime_error( + "Provided task description " + "does not have expected type 'CentauroSteeringTask'"); + } + + TaskPtr constructTask() override { + sot_steering_ = boost::make_shared( + ci_steering_->wheel_name, _model, _ctx->params()->getControlPeriod(), ci_steering_->max_steering_speed); + + return sot_steering_; + } + + private: + boost::shared_ptr sot_steering_; + CentauroSteeringTask::Ptr ci_steering_; +}; + +CARTESIO_REGISTER_OPENSOT_TASK_PLUGIN(CentauroSteeringOpenSot, CentauroSteering); + +struct WheelRollingTask : public TaskDescriptionImpl { + CARTESIO_DECLARE_SMART_PTR(WheelRollingTask) + + std::string wheel_name; + double wheel_radius; + Eigen::Vector3d contact_plane_normal; + bool include_z_axis; + + WheelRollingTask(YAML::Node task_node, Context::ConstPtr ctx); +}; + +WheelRollingTask::WheelRollingTask(YAML::Node task_node, Context::ConstPtr ctx) + : TaskDescriptionImpl(task_node, std::move(ctx), "rolling_" + task_node["wheel_name"].as(), 2), + contact_plane_normal(0, 0, 1.), + include_z_axis(false) { + wheel_name = task_node["wheel_name"].as(); + + wheel_radius = task_node["wheel_radius"].as(); + + if (auto n = task_node["contact_plane_normal"]) { + auto contact_plane_normal_std = n.as>(); + + if (contact_plane_normal_std.size() != 3) { + throw std::runtime_error("contact_plane_normal size() must be 3"); + } + + contact_plane_normal = Eigen::Vector3d::Map(contact_plane_normal_std.data()); + } + + if (auto n = task_node["include_z_axis"]) { + include_z_axis = n.as(); + } +} + +CARTESIO_REGISTER_TASK_PLUGIN(WheelRollingTask, WheelRolling) + +class WheelRollingOpenSot : public OpenSotTaskAdapter { + public: + WheelRollingOpenSot(const TaskDescription::Ptr& task_desc, Context::ConstPtr ctx) + : OpenSotTaskAdapter(task_desc, std::move(ctx)) { + ci_rolling_ = std::dynamic_pointer_cast(task_desc); + + if (!ci_rolling_) + throw std::runtime_error( + "Provided task description " + "does not have expected type 'WheelRollingTask'"); + } + + TaskPtr constructTask() override { + auto sot_rolling = boost::make_shared( + ci_rolling_->wheel_name, ci_rolling_->wheel_radius, *_model, ci_rolling_->include_z_axis); + + return sot_rolling; + } + + private: + WheelRollingTask::Ptr ci_rolling_; +}; + +CARTESIO_REGISTER_OPENSOT_TASK_PLUGIN(WheelRollingOpenSot, WheelRolling); diff --git a/src/lib/cartesio_server.cpp b/src/lib/cartesio_server.cpp index a6202db..5cdda8b 100644 --- a/src/lib/cartesio_server.cpp +++ b/src/lib/cartesio_server.cpp @@ -62,8 +62,8 @@ CartesIOServer::CartesIOServer(const ros::NodeHandle& node_handle, const ros::No // Initialize control servers execute_all_poses_srv_ = nh_.advertiseService("execute_all_poses", &CartesIOServer::executeAllPosesSrvCb, this); - // execute_mirrored_pose_srv_ = - // nh_.advertiseService("execute_mirrored_pose", &CartesIOServer::executeMirroredPoseSrvCb, this); + execute_all_locked_poses_srv_ = + nh_.advertiseService("execute_all_locked_poses", &CartesIOServer::executeAllLockedPosesSrvCb, this); } auto CartesIOServer::executeAllPosesSrvCb(roport::ExecuteAllPoses::Request& req, @@ -74,44 +74,21 @@ auto CartesIOServer::executeAllPosesSrvCb(roport::ExecuteAllPoses::Request& req, for (int i = 0; i < group_names_.size(); ++i) { for (int j = 0; j < req.group_names.size(); ++j) { if (req.group_names[j] == group_names_[i]) { - geometry_msgs::TransformStamped current_transform; - if (!getTransform(i, current_transform)) { + geometry_msgs::Pose current_pose; + if (!getCurrentPoseWithIndex(i, current_pose)) { return false; } - geometry_msgs::Pose current_pose; - current_pose.position.x = current_transform.transform.translation.x; - current_pose.position.y = current_transform.transform.translation.y; - current_pose.position.z = current_transform.transform.translation.z; - current_pose.orientation.x = current_transform.transform.rotation.x; - current_pose.orientation.y = current_transform.transform.rotation.y; - current_pose.orientation.z = current_transform.transform.rotation.z; - current_pose.orientation.w = current_transform.transform.rotation.w; - ROS_INFO_STREAM(current_pose); - // Initialize the goal pose geometry_msgs::Pose goal_pose; - - // Transfer the given pose to the reference frame - if (req.goal_type == req.GLOBAL) { - // The given pose is already in the reference frame - goal_pose = req.goals.poses[j]; - } else if (req.goal_type == req.LOCAL_ALIGNED) { - // The given pose is relative to the local aligned frame having the same orientation as the reference frame - localAlignedPoseToGlobalPose(req.goals.poses[j], current_pose, goal_pose, true); - } else { - // The given pose is relative to the local frame - localPoseToGlobalPose(req.goals.poses[j], current_pose, goal_pose); - } + toGlobalPose(req.goal_type, current_pose, req.goals.poses[j], goal_pose); // Build trajectory to reach the goal cartesian_interface::ReachPoseActionGoal action_goal; buildActionGoal(i, goal_pose, action_goal); if (req.stamps.size() == req.group_names.size() && req.stamps[j] > 0) { updateStamp(req.stamps[j], action_goal); - action_goals.insert({i, action_goal}); - } else { - action_goals.insert({i, action_goal}); } + action_goals.insert({i, action_goal}); } } } @@ -123,6 +100,53 @@ auto CartesIOServer::executeAllPosesSrvCb(roport::ExecuteAllPoses::Request& req, return true; } +auto CartesIOServer::executeAllLockedPosesSrvCb(roport::ExecuteAllLockedPoses::Request& req, + roport::ExecuteAllLockedPoses::Response& resp) -> bool { + std::map action_goals; + + int reference_index = -1; + geometry_msgs::Pose current_reference_pose; + geometry_msgs::Pose goal_reference_pose; + for (int j = 0; j < req.group_names.size(); ++j) { + int i = getIndex(group_names_, req.group_names[j]); + if (i < 0) { + ROS_ERROR_STREAM("No " << group_names_[j] << " group defined"); + return false; + } + + geometry_msgs::Pose goal_pose; + if (j == 0) { + if (!getCurrentPoseWithIndex(i, current_reference_pose)) { + return false; + } + reference_index = i; + toGlobalPose(req.goal_type, current_reference_pose, req.goal, goal_pose); + goal_reference_pose = goal_pose; + } else { + geometry_msgs::Pose current_pose; + if (!getCurrentPoseWithIndex(i, current_pose)) { + return false; + } + getGoalPoseWithReference(reference_index, current_reference_pose, goal_reference_pose, i, current_pose, + goal_pose); + } + // Build trajectory to reach the goal + cartesian_interface::ReachPoseActionGoal action_goal; + buildActionGoal(i, goal_pose, action_goal); + if (req.stamp > 0) { + updateStamp(req.stamp, action_goal); + } + action_goals.insert({i, action_goal}); + } + + if (executeGoals(action_goals)) { + resp.result_status = resp.SUCCEEDED; + } else { + resp.result_status = resp.FAILED; + } + return true; +} + bool CartesIOServer::getTransform(const int& index, geometry_msgs::TransformStamped& transform) { try { // The transform derived by lookupTransform is T_rc, i.e., from the reference to the controlled frame. @@ -134,97 +158,46 @@ bool CartesIOServer::getTransform(const int& index, geometry_msgs::TransformStam } } -// auto CartesIOServer::executeMirroredPoseSrvCb(roport::ExecuteMirroredPose::Request& req, -// roport::ExecuteMirroredPose::Response& resp) -> bool { -// int ref_index = getGroupIndex(req.reference_group); -// if (ref_index < 0) { -// throw std::runtime_error("Reference group does not exist"); -// } -// int mirror_index = getGroupIndex(req.mirror_group); -// if (mirror_index < 0) { -// throw std::runtime_error("Mirror group does not exist"); -// } -// -// std::map trajectories; -// trajectory_msgs::JointTrajectory trajectory; -// geometry_msgs::PoseStamped current_pose_wrt_base = interfaces_[ref_index]->getCurrentPose(); -// geometry_msgs::PoseStamped goal_pose_wrt_base; -// goal_pose_wrt_base.header = current_pose_wrt_base.header; -// -// // Transfer given goal pose to the base frame -// if (req.goal_type == req.BASE_ABS) { -// goal_pose_wrt_base.pose = req.goal; -// } else if (req.goal_type == req.BASE_REL) { -// geometry_msgs::PoseStamped transform_wrt_local_base; -// transform_wrt_local_base.pose = req.goal; -// relativePoseToAbsolutePose(transform_wrt_local_base, current_pose_wrt_base, goal_pose_wrt_base); -// } else { -// geometry_msgs::PoseStamped goal_pose_wrt_eef; -// goal_pose_wrt_eef.pose = req.goal; -// goal_pose_wrt_eef.header.frame_id = interfaces_[ref_index]->getEndEffectorLink(); -// goal_pose_wrt_base = -// tf_buffer_.transform(goal_pose_wrt_eef, interfaces_[ref_index]->getPlanningFrame(), ros::Duration(1)); -// } -// -// // Build trajectory to reach the goal -// if (!buildTrajectory(*interfaces_[ref_index], goal_pose_wrt_base, trajectory, req.is_cartesian)) { -// ROS_ERROR("RoPort: Building trajectory failed"); -// resp.result_status = resp.FAILED; -// return true; -// } -// -// trajectory_msgs::JointTrajectory updated_trajectory; -// if (req.stamp > 0) { -// updateTrajectoryStamp(trajectory, req.stamp, updated_trajectory); -// } else { -// updated_trajectory = trajectory; -// } -// trajectories.insert({ref_index, updated_trajectory}); -// -// trajectory_msgs::JointTrajectory mirrored_trajectory; -// getMirroredTrajectory(*interfaces_[mirror_index], updated_trajectory, req.mirror_vector, mirrored_trajectory); -// trajectories.insert({mirror_index, mirrored_trajectory}); -// -// if (executeTrajectories(trajectories)) { -// resp.result_status = resp.SUCCEEDED; -// } else { -// resp.result_status = resp.FAILED; -// } -// return true; -// } -// -// void CartesIOServer::getMirroredTrajectory(MoveGroupInterface& move_group, -// trajectory_msgs::JointTrajectory trajectory, -// std::vector mirror_vector, -// trajectory_msgs::JointTrajectory& mirrored_trajectory) { -// mirrored_trajectory.joint_names = move_group.getActiveJoints(); -// mirrored_trajectory.header = trajectory.header; -// for (size_t i = 0; i < trajectory.points.size(); ++i) { -// trajectory_msgs::JointTrajectoryPoint p; -// p = trajectory.points[i]; -// ROS_ASSERT(p.positions.size() == mirror_vector.size()); -// for (size_t j = 0; j < p.positions.size(); ++j) { -// p.positions[j] *= mirror_vector[j]; -// if (!p.velocities.empty()) { -// p.velocities[j] *= mirror_vector[j]; -// } -// if (!p.accelerations.empty()) { -// p.accelerations[j] *= mirror_vector[j]; -// } -// if (!p.effort.empty()) { -// p.effort[j] *= mirror_vector[j]; -// } -// } -// mirrored_trajectory.points.push_back(p); -// } -// } +bool CartesIOServer::getCurrentPoseWithIndex(const int& index, geometry_msgs::Pose& pose) { + geometry_msgs::TransformStamped transform; + if (!getTransform(index, transform)) { + return false; + } + + pose.position.x = transform.transform.translation.x; + pose.position.y = transform.transform.translation.y; + pose.position.z = transform.transform.translation.z; + pose.orientation.x = transform.transform.rotation.x; + pose.orientation.y = transform.transform.rotation.y; + pose.orientation.z = transform.transform.rotation.z; + pose.orientation.w = transform.transform.rotation.w; + return true; +} + +void CartesIOServer::getGoalPoseWithReference(const int& ref_idx, + const geometry_msgs::Pose& curr_ref_pose, + const geometry_msgs::Pose& goal_ref_pose, + const int& idx, + const geometry_msgs::Pose& curr_pose, + geometry_msgs::Pose& goal_pose) { + // TODO release this constraint + ROS_ASSERT(reference_frames_[ref_idx] == reference_frames_[idx]); + Eigen::Matrix4d trans_b_rc, trans_b_rg, trans_b_c; + geometryPoseToEigenMatrix(curr_ref_pose, trans_b_rc); + geometryPoseToEigenMatrix(goal_ref_pose, trans_b_rg); + geometryPoseToEigenMatrix(curr_pose, trans_b_c); + + // T_B_G = T_B_RG * T_RC_B * T_B_C + auto trans_b_g = trans_b_rg * trans_b_rc.inverse() * trans_b_c; + eigenMatrixToGeometryPose(trans_b_g, goal_pose); +} void CartesIOServer::buildActionGoal(const int& index, const geometry_msgs::Pose& goal_pose, cartesian_interface::ReachPoseActionGoal& action_goal) { action_goal.header.frame_id = reference_frames_[index]; action_goal.goal.frames.push_back(goal_pose); - action_goal.goal.time.push_back(1.); + action_goal.goal.time.push_back(10.); action_goal.goal.incremental = false; } diff --git a/src/rotools/hpp/core/core.py b/src/rotools/hpp/core/core.py index 17da952..a473a33 100644 --- a/src/rotools/hpp/core/core.py +++ b/src/rotools/hpp/core/core.py @@ -1,12 +1,12 @@ class Object(object): - rootJointType = 'freeflyer' + rootJointType = "freeflyer" urdfSuffix = "" srdfSuffix = "" def __init__(self, object_name, pkg_name): - self.__setattr__('urdfName', object_name) - self.__setattr__('packageName', pkg_name) - self.__setattr__('meshPackageName', pkg_name) + self.__setattr__("urdfName", object_name) + self.__setattr__("packageName", pkg_name) + self.__setattr__("meshPackageName", pkg_name) class Environment(object): @@ -14,21 +14,22 @@ class Environment(object): srdfSuffix = "" def __init__(self, env_name, pkg_name): - self.__setattr__('urdfName', env_name) - self.__setattr__('packageName', pkg_name) - self.__setattr__('meshPackageName', pkg_name) + self.__setattr__("urdfName", env_name) + self.__setattr__("packageName", pkg_name) + self.__setattr__("meshPackageName", pkg_name) class Model(object): - - def __init__(self, name, pkg_name, urdf_name='', srdf_name='', surface='', handle=''): + def __init__( + self, name, pkg_name, urdf_name="", srdf_name="", surface="", handle="" + ): self._name = name self._pkg_name = pkg_name - if urdf_name == '': + if urdf_name == "": self._urdf_name = name else: self._urdf_name = urdf_name - if srdf_name == '': + if srdf_name == "": self._srdf_name = name else: self._srdf_name = srdf_name @@ -61,8 +62,16 @@ def handle(self): class Gripper(object): - def __init__(self, robot_model, name, fingers, finger_joints, joint_values): + """ + + Args: + robot_model: + name: + fingers: + finger_joints: + joint_values: + """ super(Gripper, self).__init__() self._dof = 2 diff --git a/src/rotools/hpp/manipulation/client.py b/src/rotools/hpp/manipulation/client.py index b7b2c29..d5d07db 100644 --- a/src/rotools/hpp/manipulation/client.py +++ b/src/rotools/hpp/manipulation/client.py @@ -2,6 +2,7 @@ from __future__ import print_function import rospy +from threading import Lock from sensor_msgs.msg import JointState from geometry_msgs.msg import Pose @@ -23,27 +24,40 @@ def __init__(self, kwargs): self.interface = interface.HPPManipulationInterface(**kwargs) - self._srv_execute_path_planning = rospy.Service('execute_path_planning', ExecutePathPlanning, - self.execute_path_planning_handle) + self._srv_execute_path_planning = rospy.Service( + "execute_path_planning", + ExecutePathPlanning, + self.execute_path_planning_handle, + ) - self._srv_execute_manipulation_planning = rospy.Service('execute_manipulation_planning', - ExecuteManipulationPlanning, - self.execute_manipulation_planning_handle) + self._srv_execute_manipulation_planning = rospy.Service( + "execute_manipulation_planning", + ExecuteManipulationPlanning, + self.execute_manipulation_planning_handle, + ) + + self._mutex = Lock() try: - js_topic = kwargs['joint_state_topic'] - self._sub_joint_state = rospy.Subscriber(js_topic, JointState, self.update_cb, buff_size=1) + js_topic = kwargs["joint_state_topic"] + self._sub_joint_state = rospy.Subscriber( + js_topic, JointState, self.js_update_cb, buff_size=1 + ) except KeyError: rospy.logwarn("Joint state topic is not provided") try: - odom_topic = kwargs['odom_topic'] - self._sub_odom = rospy.Subscriber(odom_topic, Odometry, self.odom_update_cb, buff_size=1) + odom_topic = kwargs["odom_topic"] + self._sub_odom = rospy.Subscriber( + odom_topic, Odometry, self.odom_update_cb, buff_size=1 + ) except KeyError: rospy.logwarn("Odom topic is not provided") try: - object_pose_topic = kwargs['object_pose_topic'] - self._sub_object_pose = rospy.Subscriber(object_pose_topic, Pose, self.update_cb, buff_size=1) + object_pose_topic = kwargs["object_pose_topic"] + self._sub_object_pose = rospy.Subscriber( + object_pose_topic, Pose, self.object_pose_update_cb, buff_size=1 + ) except KeyError: rospy.logwarn("Object pose topic is not provided") @@ -51,21 +65,31 @@ def execute_path_planning_handle(self, req): resp = ExecutePathPlanningResponse() # req = ExecutePathPlanningRequest() base_current_global_pose = self.interface.get_current_base_global_pose() - base_goal_pose = self._to_global_pose(req.base_goal_pose, req.base_goal_type, base_current_global_pose) - ok = self.interface.make_approaching_plan(base_goal_pose, req.joint_goal_state, req.base_pos_tolerance, - req.base_ori_tolerance) + base_goal_pose = self._to_global_pose( + req.base_goal_pose, req.base_goal_type, base_current_global_pose + ) + ok = self.interface.approach( + base_goal_pose, + req.joint_goal_state, + req.base_pos_tolerance, + req.base_ori_tolerance, + ) resp.result_status = resp.SUCCEEDED if ok else resp.FAILED return resp def execute_manipulation_planning_handle(self, req): resp = ExecuteManipulationPlanningResponse() base_current_global_pose = self.interface.get_current_base_global_pose() - object_goal_pose = self._to_global_pose(req.object_goal_pose, req.object_goal_pose_type, - base_current_global_pose) - base_goal_pose = self._to_global_pose(req.base_goal_pose, req.base_goal_pose_type, base_current_global_pose) - ok = self.interface.make_grasping_plan(base_goal_pose, req.joint_goal_state, object_goal_pose, - req.base_pos_tolerance, req.base_ori_tolerance, req.object_pos_tolerance, - req.object_ori_tolerance) + object_goal_pose = self._to_global_pose( + req.object_goal_pose, req.object_goal_pose_type, base_current_global_pose + ) + # base_goal_pose = self._to_global_pose(req.base_goal_pose, req.base_goal_pose_type, base_current_global_pose) + ok = self.interface.grasp( + req.joint_goal_state, + object_goal_pose, + req.object_pos_tolerance, + req.object_ori_tolerance, + ) resp.result_status = resp.SUCCEEDED if ok else resp.FAILED return resp @@ -79,8 +103,23 @@ def _to_global_pose(self, pose, pose_type, reference_pose): else: raise NotImplementedError - def update_cb(self, msg): - self.interface.update_current_config(msg) + def js_update_cb(self, msg): + self._mutex.acquire() + try: + self.interface.update_current_config(msg) + finally: + self._mutex.release() def odom_update_cb(self, msg): - self.interface.update_current_config(msg) + self._mutex.acquire() + try: + self.interface.update_current_config(msg) + finally: + self._mutex.release() + + def object_pose_update_cb(self, msg): + self._mutex.acquire() + try: + self.interface.update_current_config(msg) + finally: + self._mutex.release() diff --git a/src/rotools/hpp/manipulation/interface.py b/src/rotools/hpp/manipulation/interface.py index e47107c..79cca8e 100644 --- a/src/rotools/hpp/manipulation/interface.py +++ b/src/rotools/hpp/manipulation/interface.py @@ -9,8 +9,15 @@ import rospy from hpp.corbaserver.manipulation.robot import Robot as Parent -from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule, \ - Constraints, ConstraintGraphFactory, Client +from hpp.corbaserver.manipulation import ( + ProblemSolver, + ConstraintGraph, + Rule, + Constraints, + ConstraintGraphFactory, + Client, + SecurityMargins, +) from hpp.corbaserver import loadServerPlugin from hpp.gepetto import PathPlayer @@ -25,31 +32,31 @@ class HPPManipulationInterface(object): - def __init__( - self, - env_name, - env_pkg_name, - env_surface, - object_name, - object_pkg_name, - object_surface, - object_handle, - robot_name, - robot_pkg_name, - robot_urdf_name, - robot_srdf_name, - robot_bound, - object_bound, - gripper_name, - fingers, - finger_joints, - finger_joint_values, - joint_cmd_topic, - base_cmd_topic, - enable_viewer=True, - reduction_ratio=0.2, - **kwargs + self, + env_name, + env_pkg_name, + env_surface, + object_name, + object_pkg_name, + object_surface, + object_handle, + robot_name, + robot_pkg_name, + robot_urdf_name, + robot_srdf_name, + robot_bound, + object_bound, + gripper_name, + fingers, + finger_joints, + finger_joint_values, + joint_cmd_topic, + base_cmd_topic, + enable_viewer=True, + reduction_ratio=0.2, + safety_margin=0.2, + **kwargs ): super(HPPManipulationInterface, self).__init__() @@ -57,30 +64,44 @@ def __init__( Client().problem.resetProblem() self._rm = Model(robot_name, robot_pkg_name, robot_urdf_name, robot_srdf_name) - self._om = Model(object_name, object_pkg_name, surface=object_surface, handle=object_handle) + self._om = Model( + object_name, object_pkg_name, surface=object_surface, handle=object_handle + ) self._em = Model(env_name, env_pkg_name, surface=env_surface) - self._gm = Gripper(self._rm, gripper_name, fingers, finger_joints, finger_joint_values) + self._gm = Gripper( + self._rm, gripper_name, fingers, finger_joints, finger_joint_values + ) - self._work_modes = namedtuple("work_modes", "idle approach grasp") - self._previous_mode = self._work_modes.idle - self._mode = self._work_modes.idle + self.Modes = namedtuple("Modes", "idle approach grasp") + self._current_mode = self.Modes.idle self._robot = self._create_robot() self._problem_solver = ProblemSolver(self._robot) - self._problem_solver.loadPlugin('manipulation-spline-gradient-based.so') + self._problem_solver.loadPlugin("manipulation-spline-gradient-based.so") # To show available path optimizers: + # ['EnforceTransitionSemantic', 'Graph-PartialShortcut', 'Graph-RandomShortcut', 'PartialShortcut', + # 'RandomShortcut', 'SimpleShortcut', 'SimpleTimeParameterization', 'SplineGradientBased_bezier1', + # 'SplineGradientBased_bezier3'] # print(self._problem_solver.getAvailable('PathOptimizer')) - self._problem_solver.addPathOptimizer('RandomShortcut') + self._problem_solver.addPathOptimizer("RandomShortcut") + + # To show available config validation methods + # (['CollisionValidation', 'JointBoundValidation']) + # print(self._problem_solver.getAvailable('ConfigValidation')) + # self._problem_solver.clearConfigValidations() self._enable_viewer = enable_viewer - self._viewer_factory = self._create_viewer_factory() # need to create this no matter if enable_viewer + self._viewer_factory = ( + self._create_viewer_factory() + ) # need to create this no matter if enable_viewer self._robot.setJointBounds("{}/root_joint".format(self._rm.name), robot_bound) self._robot.setJointBounds("{}/root_joint".format(self._om.name), object_bound) # An absolute value, if the threshold is surpassed, will raise the error `A configuration has no node` - self._problem_solver.setErrorThreshold(3e-3) + self._error_threshold = 3.0e-3 + self._problem_solver.setErrorThreshold(self._error_threshold) self._problem_solver.setMaxIterProjection(80) # Use this one and/or the next to limit solving time. MaxIterPathPlanning is a fairly large value if not set. @@ -99,14 +120,16 @@ def __init__( self._joint_names = [] self._q_current = self._robot.getCurrentConfig() - self._q_goal = self._q_current[::] self._lock_hand = self._create_locks() self._graph_id = 0 + self._safety_margin = safety_margin self._constraint_graph = self._create_constraint_graph() - self._joint_cmd_publisher = rospy.Publisher(joint_cmd_topic, JointState, queue_size=1) + self._joint_cmd_publisher = rospy.Publisher( + joint_cmd_topic, JointState, queue_size=1 + ) self._base_cmd_publisher = rospy.Publisher(base_cmd_topic, Twist, queue_size=1) self._time_step = 0.002 @@ -119,10 +142,14 @@ def __init__( self._object_p_tol = 0.01 self._object_o_tol = 0.02 - def _create_robot(self, root_joint_type='planar'): + def _create_robot(self, root_joint_type="planar"): class CompositeRobot(Parent): - urdfFilename = "package://{}/urdf/{}.urdf".format(self._rm.pkg_name, self._rm.urdf_name) - srdfFilename = "package://{}/srdf/{}.srdf".format(self._rm.pkg_name, self._rm.srdf_name) + urdfFilename = "package://{}/urdf/{}.urdf".format( + self._rm.pkg_name, self._rm.urdf_name + ) + srdfFilename = "package://{}/srdf/{}.srdf".format( + self._rm.pkg_name, self._rm.srdf_name + ) rootJointType = root_joint_type # \param compositeName name of the composite robot that will be built later, @@ -130,11 +157,21 @@ class CompositeRobot(Parent): # \param load whether to actually load urdf files. Set to false if you only # want to initialize a corba client to an already initialized problem. # \param rootJointType type of root joint among ("freeflyer", "planar", "anchor"), - def __init__(self, composite_name, robot_name, load=True, - root_joint="planar", **kwargs): - Parent.__init__(self, composite_name, robot_name, root_joint, load, **kwargs) - - robot = CompositeRobot('{}-{}'.format(self._rm.name, self._om.name), self._rm.name) + def __init__( + self, + composite_name, + robot_name, + load=True, + root_joint="planar", + **kwargs + ): + Parent.__init__( + self, composite_name, robot_name, root_joint, load, **kwargs + ) + + robot = CompositeRobot( + "{}-{}".format(self._rm.name, self._om.name), self._rm.name + ) return robot def _create_viewer_factory(self): @@ -149,146 +186,260 @@ def _create_locks(self): lock_hand = self._gm.fingers for i in range(self._gm.dof): self._problem_solver.createLockedJoint( - self._gm.fingers[i], '{}/{}'.format(self._rm.name, self._gm.joints[i]), [self._gm.values[i], ] + self._gm.fingers[i], + "{}/{}".format(self._rm.name, self._gm.joints[i]), + [ + self._gm.values[i], + ], ) return lock_hand - def _create_constraint_graph(self): + def _create_constraint_graph(self, initialize=True): + """When new graph is created, we increase the count of graph_id to prevent naming ambiguous. + Creating multiple graphs only consume a little memory and beneficial for increase planning success rate. + + Args: + initialize: bool If true, the created graph will be initialized. + + Returns: + Created constraint graph. + """ self._graph_id += 1 - graph_name = 'graph_{}'.format(self._graph_id) - constraint_graph = ConstraintGraph(self._robot, graph_name) - rospy.loginfo('Created new constraint graph: {}'.format(graph_name)) - factory = ConstraintGraphFactory(constraint_graph) - factory.setGrippers(["{}/{}".format(self._rm.name, self._gm.name), ]) - factory.environmentContacts(["{}/{}".format(self._em.name, self._em.surface), ]) - factory.setObjects([self._om.name, ], - [["{}/{}".format(self._om.name, self._om.handle), ], ], - [["{}/{}".format(self._om.name, self._om.surface), ], ]) - factory.setRules([Rule([".*"], [".*"], True), ]) + graph_name = "graph_{}".format(self._graph_id) + graph = ConstraintGraph(self._robot, graph_name) + rospy.logdebug("Created new constraint graph: {}".format(graph_name)) + factory = ConstraintGraphFactory(graph) + factory.setGrippers( + [ + "{}/{}".format(self._rm.name, self._gm.name), + ] + ) + factory.environmentContacts( + [ + "{}/{}".format(self._em.name, self._em.surface), + ] + ) + factory.setObjects( + [ + self._om.name, + ], + [ + [ + "{}/{}".format(self._om.name, self._om.handle), + ], + ], + [ + [ + "{}/{}".format(self._om.name, self._om.surface), + ], + ], + ) + factory.setRules( + [ + Rule([".*"], [".*"], True), + ] + ) # factory.setPreplacementDistance('{}'.format(self._om.name), 0.1) factory.generate() - constraint_graph.addConstraints(graph=True, constraints=Constraints(numConstraints=self._lock_hand)) - constraint_graph.initialize() - return constraint_graph - - def _update_constraints(self): - """Update the constraints if 1) Working mode turned from other modes to the grasp mode. In this mode, - we need to add extra constraints to the edges such that the base do not move during grasping. 2) Working - mode turned from grasping to other modes, where we need to create a new graph since the original constrained - graph cannot be used. When new graph is created, we increase the count of graph_id to prevent naming ambiguous. + graph.addConstraints( + graph=True, constraints=Constraints(numConstraints=self._lock_hand) + ) + # graph.display(open=False) + security_margins = SecurityMargins( + self._problem_solver, factory, [self._rm.name, self._om.name] + ) + security_margins.setSecurityMarginBetween( + self._rm.name, self._om.name, self._safety_margin + ) + security_margins.apply() + if initialize: + graph.initialize() + return graph + + def _update_constraint_graph(self): + """Update the constraints according to working modes. In the grasp mode, + we need to add extra constraints to the edges such that the base do not move during grasping. Returns: None """ - if self._previous_mode == self._work_modes.grasp and self._mode != self._work_modes.grasp: - self._constraint_graph = self._create_constraint_graph() - if self._previous_mode != self._work_modes.grasp and self._mode == self._work_modes.grasp: - self._problem_solver.createLockedJoint("fix_base", "{}/root_joint".format(self._rm.name), [0, 0, 1, 0]) + if self._current_mode == self.Modes.grasp: + self._constraint_graph = self._create_constraint_graph(initialize=False) + self._problem_solver.createLockedJoint( + "fix_base", "{}/root_joint".format(self._rm.name), [0, 0, 1, 0] + ) for e in self._constraint_graph.edges.keys(): - self._constraint_graph.addConstraints(edge=e, constraints=Constraints(numConstraints=["fix_base"])) + self._constraint_graph.addConstraints( + edge=e, constraints=Constraints(numConstraints=["fix_base"]) + ) self._constraint_graph.initialize() - self._previous_mode = self._mode + else: + self._constraint_graph = self._create_constraint_graph() def update_current_config(self, msg): if isinstance(msg, JointState): self._set_robot_joint_state_config(self._q_current, msg, add_name=True) - rospy.loginfo_once('First joint state received') + rospy.loginfo_once("First joint state received") elif isinstance(msg, Odometry): - self._set_robot_base_config(self._q_current, msg) - rospy.loginfo_once('First odom received') + self._set_planar_robot_base_config(self._q_current, msg) + rospy.loginfo_once("First odom received") elif isinstance(msg, Pose): self._set_object_config(self._q_current, msg) - rospy.loginfo_once('First object pose received') + rospy.loginfo_once("First object pose received") else: - rospy.logerr("Msg is not of type JointState/Odometry/Pose: {}".format(type(msg))) + rospy.logerr( + "Msg is not of type JointState/Odometry/Pose: {}".format(type(msg)) + ) def get_current_base_global_pose(self): - return self._get_robot_base_pose(self._q_current) + return self._get_planar_robot_base_pose(self._q_current) - def _make_plan(self): - while not self._check_goal_reached(): - q_goal = self._q_goal[::] - q_current = self._q_current[::] - if self._mode == self._work_modes.grasp: - q_goal[:4] = q_current[:4] + def _implement_plan(self, q_goal): + """Given the goal configuration and dynamically retrieved initial configuration, this function plans a + valid path and implements the plan. - self._problem_solver.addGoalConfig(q_goal) - self._problem_solver.setInitialConfig(q_current) + Args: + q_goal: list[double] Complete configuration of the goal state. - rospy.loginfo("Current configuration:\n{}".format(["{0:0.8f}".format(i) for i in q_current])) - rospy.loginfo("Goal configuration:\n{}".format(["{0:0.8f}".format(i) for i in q_goal])) + Returns: + bool True if succeeded, false otherwise. + """ + q_init = self._regulate_lock_hand_joints(self._q_current) + # q_init = self._project_config_if_necessary(self._q_current) + self._problem_solver.addGoalConfig(q_goal) + self._problem_solver.setInitialConfig(q_init) + + rospy.logdebug( + "Init configuration:\n{}".format(["{0:0.8f}".format(i) for i in q_init]) + ) + rospy.logdebug( + "Goal configuration:\n{}".format(["{0:0.8f}".format(i) for i in q_goal]) + ) + + self._update_constraint_graph() + try: + time_spent = self._problem_solver.solve() + rospy.loginfo("Plan solved in {}h-{}m-{}s-{}ms".format(*time_spent)) + except BaseException as e: + rospy.logwarn("Failed to solve due to {}".format(e)) + self._problem_solver.resetGoalConfigs() + return False + + if self._enable_viewer: + viewer = self._viewer_factory.createViewer() + viewer(q_init) + path_player = PathPlayer(viewer) + path_player(self._last_path_id) + + # Get waypoints information + grasp_stamps = [] + release_stamps = [] + if self._current_mode == self.Modes.grasp: + # waypoints is a list of configurations, each configuration corresponds to a node (or state) + # in the graph, time stamp to reach that state is recorded in times. + waypoints, times = self._problem_solver.getWaypoints(self._last_path_id) + temp = [[wp, t] for wp, t in zip(waypoints, times)] + for i in range(len(temp) - 1): + if self._constraint_graph.getNode( + temp[i][0] + ) == "free" and "grasp" in self._constraint_graph.getNode( + temp[i + 1][0] + ): + grasp_stamp = temp[i + 1][1] + grasp_stamps.append(grasp_stamp) + rospy.loginfo("Will grasp at {:.4f} s".format(grasp_stamp)) + if self._constraint_graph.getNode( + temp[i + 1][0] + ) == "free" and "grasp" in self._constraint_graph.getNode(temp[i][0]): + release_stamps.append(temp[i + 1][1]) + rospy.loginfo("Will release at {:.4f} s".format(temp[i + 1][1])) + + path_length = self._problem_solver.pathLength(self._last_path_id) + msgs = [] + close_gripper = False + for t in np.arange(0, path_length, self._time_step): + j_q = self._problem_solver.configAtParam(self._last_path_id, t) + j_dq = self._problem_solver.derivativeAtParam(self._last_path_id, 1, t) + for g_s in grasp_stamps: + if t <= g_s < t + self._time_step: + close_gripper = True + break + for r_s in release_stamps: + if t <= r_s < t + self._time_step: + close_gripper = False + break + msgs.append(self._derive_command_msgs(j_q, j_dq, close_gripper)) + j_q = self._problem_solver.configAtParam(self._last_path_id, path_length) + j_dq = self._problem_solver.derivativeAtParam( + self._last_path_id, 1, path_length + ) + joint_cmd, base_cmd = self._derive_command_msgs(j_q, j_dq) + + global_start = time.time() + r = rospy.Rate(1.0 / self._time_step * self._reduction_ratio) + for j_cmd, base_cmd in msgs: + self._publish_planning_results(j_cmd, base_cmd) + r.sleep() + + self._publish_planning_results(joint_cmd, base_cmd) + rospy.Rate( + 1.0 / (path_length % self._time_step) * self._reduction_ratio + ).sleep() + rospy.loginfo( + "Path length: {:.4f}; actual elapsed time: {:.4f}".format( + path_length, (time.time() - global_start) * self._reduction_ratio + ) + ) + self._stop_base() + self._problem_solver.resetGoalConfigs() + return True - self._update_constraints() - try: - time_spent = self._problem_solver.solve() - rospy.loginfo('Plan solved in {}h-{}m-{}s-{}ms'.format(*time_spent)) - except BaseException as e: - rospy.logwarn('Failed to solve due to {}'.format(e)) - self._problem_solver.resetGoalConfigs() - self._mode = self._work_modes.idle - return False + def _project_config_if_necessary(self, config, node="free"): + """Project the configuration to the given node of the constraint graph to get a valid configuration. - if self._enable_viewer: - viewer = self._viewer_factory.createViewer() - viewer(q_current) - path_player = PathPlayer(viewer) - path_player(self._last_path_id) - - # Get waypoints information - grasp_stamps = [] - release_stamps = [] - if self._mode == self._work_modes.grasp: - # waypoints is a list of configurations, each configuration corresponds to a node (or state) - # in the graph, time stamp to reach that state is recorded in times. - waypoints, times = self._problem_solver.getWaypoints(self._last_path_id) - temp = [[wp, t] for wp, t in zip(waypoints, times)] - for i in range(len(temp) - 1): - if self._constraint_graph.getNode(temp[i][0]) == 'free' and \ - 'grasp' in self._constraint_graph.getNode(temp[i + 1][0]): - grasp_stamp = temp[i + 1][1] - grasp_stamps.append(grasp_stamp) - rospy.loginfo('Will grasp at {:.4f} s'.format(grasp_stamp)) - if self._constraint_graph.getNode(temp[i + 1][0]) == 'free' and \ - 'grasp' in self._constraint_graph.getNode(temp[i][0]): - release_stamps.append(temp[i + 1][1]) - rospy.loginfo('Will release at {:.4f} s'.format(temp[i + 1][1])) - - path_length = self._problem_solver.pathLength(self._last_path_id) - msgs = [] - close_gripper = False - for t in np.arange(0, path_length, self._time_step): - j_q = self._problem_solver.configAtParam(self._last_path_id, t) - j_dq = self._problem_solver.derivativeAtParam(self._last_path_id, 1, t) - for g_s in grasp_stamps: - if t <= g_s < t + self._time_step: - close_gripper = True - break - for r_s in release_stamps: - if t <= r_s < t + self._time_step: - close_gripper = False - break - msgs.append(self._derive_command_msgs(j_q, j_dq, close_gripper)) - j_q = self._problem_solver.configAtParam(self._last_path_id, path_length) - j_dq = self._problem_solver.derivativeAtParam(self._last_path_id, 1, path_length) - joint_cmd, base_cmd = self._derive_command_msgs(j_q, j_dq) - - global_start = time.time() - r = rospy.Rate(1. / self._time_step * self._reduction_ratio) - for j_cmd, base_cmd in msgs: - self._publish_planning_results(j_cmd, base_cmd) - r.sleep() - - self._publish_planning_results(joint_cmd, base_cmd) - rospy.Rate(1. / (path_length % self._time_step) * self._reduction_ratio).sleep() - rospy.loginfo('Path length: {:.4f}; actual elapsed time: {:.4f}'.format( - path_length, (time.time() - global_start) * self._reduction_ratio)) - self._stop_base() - self._problem_solver.resetGoalConfigs() + Args: + config: list[double] Complete configuration to project. + node: str Name of the node. - self._mode = self._work_modes.idle - return True + Returns: + list[double] Projected configuration. + """ + try: + name = self._constraint_graph.getNode(config) + assert name == node, print("{} is not {}".format(name, node)) + return config + except BaseException as e: + rospy.logdebug(e) + res, config_proj, _ = self._constraint_graph.applyNodeConstraints( + node, config + ) + self._log_config("Original configuration", config, show_all=True) + self._log_config("Projected configuration", config_proj, show_all=True) + if res: + return config_proj + else: + raise ValueError("Failed to project config into {} node".format(node)) - def make_approaching_plan(self, base_goal_pose, joint_goal_state, base_pos_tol, base_ori_tol): + def _regulate_lock_hand_joints(self, config): + """Manually set the locked hand joints the values defined with the constraint. + + Args: + config: list[double] Complete configuration. + + Returns: + list[double] Regulated configuration. + """ + regulated_configs = config[::] + for joint_name in self._joint_names: + if joint_name in self._gm.joints: + rank = self._robot.rankInConfiguration[ + "{}/{}".format(self._rm.name, joint_name) + ] + idx = self._gm.joints.index(joint_name) + regulated_configs[rank] = self._gm.values[idx] + return regulated_configs + + def approach(self, base_goal_pose, joint_goal_state, base_pos_tol, base_ori_tol): """Make a plan for the robot to approach the given base goal pose and joint goal state. Args: @@ -300,33 +451,35 @@ def make_approaching_plan(self, base_goal_pose, joint_goal_state, base_pos_tol, Returns: bool If success, return True, False otherwise. """ - self._q_goal = self._q_current[::] - odom = Odometry() - odom.pose.pose = base_goal_pose - self._set_robot_base_config(self._q_goal, odom) - self._set_robot_joint_state_config(self._q_goal, joint_goal_state) + q_goal = self._regulate_lock_hand_joints(self._q_current) + # q_goal = self._project_config_if_necessary(self._q_current[::]) + goal_odom = Odometry() + goal_odom.pose.pose = base_goal_pose + self._set_planar_robot_base_config(q_goal, goal_odom) + self._set_robot_joint_state_config(q_goal, joint_goal_state) self._base_p_tol = base_pos_tol if base_pos_tol > 0 else self._base_p_tol self._base_o_tol = base_ori_tol if base_ori_tol > 0 else self._base_o_tol - self._mode = self._work_modes.approach - return self._make_plan() + self._current_mode = self.Modes.approach + while not self._check_approaching_goal_reached(q_goal): + if not self._implement_plan(q_goal): + return False + self._current_mode = self.Modes.idle + return True - def make_grasping_plan(self, base_goal_pose, joint_goal_state, object_goal_pose, base_pos_tol, base_ori_tol, - object_pos_tol, object_ori_tol): + def grasp(self, joint_goal_state, object_goal_pose, object_pos_tol, object_ori_tol): """Make a grasping plan. Args: - base_goal_pose: Pose Goal pose of the base in the global frame. joint_goal_state: JointState Goal joint state of the robot joints. object_goal_pose: Pose Goal pose of the object in the global frame. - base_pos_tol: double Position tolerance of the base. - base_ori_tol: double Orientation tolerance of the base. object_pos_tol: double Position tolerance of the object. object_ori_tol: double Orientation tolerance of the object. Returns: - True if success, False otherwise. + True if succeed, False otherwise. """ - self._q_goal = self._q_current[::] + q_goal = self._regulate_lock_hand_joints(self._q_current) + # q_goal = self._project_config_if_necessary(self._q_current[::]) # TODO Since we fix the base during grasping, the base goal is currently disabled. # It shall be activated if we have accurate locomotion. # odom = Odometry() @@ -334,33 +487,65 @@ def make_grasping_plan(self, base_goal_pose, joint_goal_state, object_goal_pose, # self._set_robot_base_config(self._q_goal, odom) # self._base_p_tol = base_pos_tol if base_pos_tol > 0 else self._base_p_tol # self._base_o_tol = base_ori_tol if base_ori_tol > 0 else self._base_o_tol - self._set_robot_joint_state_config(self._q_goal, joint_goal_state) - self._set_object_config(self._q_goal, object_goal_pose) - self._object_p_tol = object_pos_tol if object_pos_tol > 0 else self._object_p_tol - self._object_o_tol = object_ori_tol if object_ori_tol > 0 else self._object_o_tol - self._mode = self._work_modes.grasp - return self._make_plan() + self._set_robot_joint_state_config(q_goal, joint_goal_state) + self._set_object_config(q_goal, object_goal_pose) + self._object_p_tol = ( + object_pos_tol if object_pos_tol > 0 else self._object_p_tol + ) + self._object_o_tol = ( + object_ori_tol if object_ori_tol > 0 else self._object_o_tol + ) + self._current_mode = self.Modes.grasp + while not self._check_grasping_goal_reached(q_goal): + # During grasping, the robot base could move due to external perturbations or the robot hitting an + # obstacle, so we need to make sure the locomotion is within the tolerance, otherwise the initial and + # goal configurations will never be connected together. If the base has moved, we use the new base pose + # as the goal pose. + if not self._check_approaching_goal_reached(q_goal, self._error_threshold): + q_goal[:4] = self._q_current[:4] + + if not self._implement_plan(q_goal): + return False + self._current_mode = self.Modes.idle + return True @property def _last_path_id(self): return self._problem_solver.numberPaths() - 1 @staticmethod - def _get_robot_base_pose(config): + def _get_planar_robot_base_pose(config): pose = Pose() pose.position.x = config[0] pose.position.y = config[1] cos_yaw = config[2] sin_yaw = config[3] yaw = math.atan2(sin_yaw, cos_yaw) - pose.orientation = common.to_ros_orientation(transform.euler_matrix(yaw, 0, 0, 'szyx')) + pose.orientation = common.to_ros_orientation( + transform.euler_matrix(yaw, 0, 0, "szyx") + ) return pose def _set_robot_joint_state_config(self, config, joint_state, add_name=False): + """Update the configuration corresponding to the robot's actuated joints using the given joint state msg. + The configuration will only be updated if the msg contains a name matching the predefined joint name. + We allow undocumented names inside the msg, which will be ignored. + + Args: + config: list[double] Complete configuration to be updated in-place. + joint_state: JointState Joint state msg of the robot. + add_name: bool Whether adding names from the msg to class member _joint_names. + This allows only the measured joint states to set the names, but not allow the goal to set them. + + Returns: + None + """ assert isinstance(joint_state, JointState) for i, joint_name in enumerate(joint_state.name): try: - rank = self._robot.rankInConfiguration['{}/{}'.format(self._rm.name, joint_name)] + rank = self._robot.rankInConfiguration[ + "{}/{}".format(self._rm.name, joint_name) + ] config[rank] = joint_state.position[i] if add_name and joint_name not in self._joint_names: self._joint_names.append(joint_name) @@ -368,20 +553,22 @@ def _set_robot_joint_state_config(self, config, joint_state, add_name=False): continue @staticmethod - def _set_robot_base_config(config, base_odom): - """Update the configurations of the robot base's pose. + def _set_planar_robot_base_config(config, base_odom): + """Update the configuration corresponding to the robot base's pose using the given odom msg. Args: - config: list[double] HPP configurations. - base_odom: Odometry msg of the robot's planar base. + config: list[double] Complete configuration to be updated in-place. + base_odom: Odometry Pose msg of the robot's planar base. Returns: - Updated configurations. + None """ assert isinstance(base_odom, Odometry) base_pose = base_odom.pose.pose config[0:2] = [base_pose.position.x, base_pose.position.y] - yaw = transform.euler_from_matrix(common.to_orientation_matrix(base_pose.orientation), 'szyx')[0] + yaw = transform.euler_from_matrix( + common.to_orientation_matrix(base_pose.orientation), "szyx" + )[0] config[2:4] = [math.cos(yaw), math.sin(yaw)] def _set_object_config(self, config, object_pose): @@ -396,34 +583,44 @@ def _set_object_config(self, config, object_pose): None """ assert isinstance(object_pose, Pose) - rank = self._robot.rankInConfiguration['{}/root_joint'.format(self._om.name)] - object_pose.position.z += 0.002 # Avoid false positive collision due to measurement error + rank = self._robot.rankInConfiguration["{}/root_joint".format(self._om.name)] + object_pose.position.z += ( + 0.002 # Avoid false positive collision due to measurement error + ) # Rotate around the y-axis of the object frame to make it X-up. new_pose_matrix = np.dot(common.sd_pose(object_pose), self._object_transform) - config[rank: rank + 7] = common.to_list(common.to_ros_pose(new_pose_matrix)) + config[rank : rank + 7] = common.to_list(common.to_ros_pose(new_pose_matrix)) def _publish_planning_results(self, joint_cmd, base_cmd): - if self._mode == self._work_modes.grasp: - if not self._check_goal_reached(): - self._joint_cmd_publisher.publish(joint_cmd) - self._base_cmd_publisher.publish(base_cmd) - else: - self._joint_cmd_publisher.publish(joint_cmd) - self._stop_base() - else: + if self._current_mode == self.Modes.grasp: + self._stop_base() self._joint_cmd_publisher.publish(joint_cmd) + else: self._base_cmd_publisher.publish(base_cmd) + self._joint_cmd_publisher.publish(joint_cmd) def _derive_command_msgs(self, j_q, j_dq, close_gripper=False): + """Derive command msgs for robot joints and the base. + + Args: + j_q: list[double] Joint position commands. + j_dq: list[double] Joint velocity commands. + close_gripper: bool If true, the gripper joints' commands will be set as 0. + Otherwise, the commands will follow the values in j_q. + + Returns: + list[JointState, Twist] Commands for the joints and the base. + """ joint_cmd = JointState() for name in self._joint_names: joint_cmd.name.append(name) - rank = self._robot.rankInConfiguration['{}/{}'.format(self._rm.name, name)] + rank = self._robot.rankInConfiguration["{}/{}".format(self._rm.name, name)] if name in self._gm.joints and close_gripper: + # TODO the 0 value may fail in different grasping scenarios joint_cmd.position.append(0) else: joint_cmd.position.append(j_q[rank]) - rank = self._robot.rankInVelocity['{}/{}'.format(self._rm.name, name)] + rank = self._robot.rankInVelocity["{}/{}".format(self._rm.name, name)] joint_cmd.velocity.append(j_dq[rank]) joint_cmd.effort.append(0) # TODO currently torque control is not supported @@ -439,39 +636,71 @@ def _stop_base(self): base_cmd = Twist() self._base_cmd_publisher.publish(base_cmd) - def _check_goal_reached(self): - """Check if the object and optionally the base has reached the goal pose. + def _check_approaching_goal_reached(self, q_goal, base_tol=None): + """Check if the base and the actuated joints have reached the goal. + + Args: + q_goal list[double] Complete goal configuration to be reached. + base_tol None/double Tolerance of the base error. Returns: True if both position and orientation errors are within tolerance. """ - if self._mode == self._work_modes.approach: - base_goal_pos = self._q_goal[:2] - base_curr_pos = self._q_current[:2] - base_goal_ori = self._q_goal[2:4] - base_curr_ori = self._q_current[2:4] - joint_curr_cfg = self._get_joint_configs(self._q_current) - joint_goal_cfg = self._get_joint_configs(self._q_goal) - return common.all_close(base_goal_pos, base_curr_pos, self._base_p_tol) & \ - common.all_close(base_goal_ori, base_curr_ori, self._base_o_tol) & \ - common.all_close(joint_curr_cfg, joint_goal_cfg, 0.1) - elif self._mode == self._work_modes.grasp: - rank = self._robot.rankInConfiguration['{}/root_joint'.format(self._om.name)] - obj_curr_pos = self._q_current[rank: rank + 3] - obj_goal_pos = self._q_goal[rank: rank + 3] - obj_curr_ori = self._q_current[rank + 3: rank + 7] - obj_goal_ori = self._q_goal[rank + 3: rank + 7] - return common.all_close(obj_goal_pos, obj_curr_pos, self._object_p_tol) & \ - common.all_close(obj_goal_ori, obj_curr_ori, self._object_o_tol) + base_curr_pos = self._q_current[:2] + base_curr_ori = self._q_current[2:4] + base_goal_pos = q_goal[:2] + base_goal_ori = q_goal[2:4] + joint_curr_cfg = self._get_joint_configs(self._q_current) + joint_goal_cfg = self._get_joint_configs(q_goal) + if isinstance(base_tol, float) and base_tol > 0: + return ( + common.all_close(base_goal_pos, base_curr_pos, base_tol) + & common.all_close(base_goal_ori, base_curr_ori, base_tol) + & common.all_close(joint_curr_cfg, joint_goal_cfg, 0.1) + ) else: - raise NotImplementedError + return ( + common.all_close(base_goal_pos, base_curr_pos, self._base_p_tol) + & common.all_close(base_goal_ori, base_curr_ori, self._base_o_tol) + & common.all_close(joint_curr_cfg, joint_goal_cfg, 0.1) + ) + + def _check_grasping_goal_reached(self, q_goal): + """Check if the object being manipulated has reached the goal pose. + + Returns: + True if both position and orientation errors are within tolerance. + """ + assert self._current_mode == self.Modes.grasp + rank = self._robot.rankInConfiguration["{}/root_joint".format(self._om.name)] + obj_curr_pos = self._q_current[rank : rank + 3] + obj_curr_ori = self._q_current[rank + 3 : rank + 7] + obj_goal_pos = q_goal[rank : rank + 3] + obj_goal_ori = q_goal[rank + 3 : rank + 7] + return common.all_close( + obj_goal_pos, obj_curr_pos, self._object_p_tol + ) & common.all_close(obj_goal_ori, obj_curr_ori, self._object_o_tol) def _get_joint_configs(self, all_configs): joint_configs = [] for joint_name in self._joint_names: try: - rank = self._robot.rankInConfiguration['{}/{}'.format(self._rm.name, joint_name)] + rank = self._robot.rankInConfiguration[ + "{}/{}".format(self._rm.name, joint_name) + ] joint_configs.append(all_configs[rank]) except KeyError: continue return joint_configs + + def _log_config(self, title, config, show_all=False): + rospy.loginfo("{}\n".format(title)) + if show_all: + rospy.loginfo("{}".format(["{:0.19f}".format(i) for i in config])) + return + + joint_states = self._get_joint_configs(config) + max_key_len = len(max(self._joint_names, key=len)) + for name, state in zip(self._joint_names, joint_states): + name_padded = "{message: <{width}}".format(message=name, width=max_key_len) + rospy.loginfo("{}: {:0.19f}".format(name_padded, state)) diff --git a/src/rotools/hpp/manipulation/minimal_test.py b/src/rotools/hpp/manipulation/minimal_test.py deleted file mode 100644 index 10248b4..0000000 --- a/src/rotools/hpp/manipulation/minimal_test.py +++ /dev/null @@ -1,392 +0,0 @@ -#!/usr/bin/env python -import math -import subprocess - -import numpy as np -from collections import namedtuple - -import rospy - -from hpp.corbaserver.manipulation.robot import Robot as Parent -from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule, \ - Constraints, ConstraintGraphFactory, Client -from hpp.corbaserver import loadServerPlugin - -from hpp.gepetto import PathPlayer -from hpp.gepetto.manipulation import ViewerFactory - - -def rotation_matrix(angle, direction, point=None): - sina = math.sin(angle) - cosa = math.cos(angle) - direction = unit_vector(direction[:3]) - # rotation matrix around unit vector - R = np.array(((cosa, 0.0, 0.0), - (0.0, cosa, 0.0), - (0.0, 0.0, cosa)), dtype=np.float64) - R += np.outer(direction, direction) * (1.0 - cosa) - direction *= sina - R += np.array(((0.0, -direction[2], direction[1]), - (direction[2], 0.0, -direction[0]), - (-direction[1], direction[0], 0.0)), - dtype=np.float64) - M = np.identity(4) - M[:3, :3] = R - if point is not None: - # rotation not around origin - point = np.array(point[:3], dtype=np.float64, copy=False) - M[:3, 3] = point - np.dot(R, point) - return M - - -def unit_vector(data, axis=None, out=None): - if out is None: - data = np.array(data, dtype=np.float64, copy=True) - if data.ndim == 1: - data /= math.sqrt(np.dot(data, data)) - return data - else: - if out is not data: - out[:] = np.array(data, copy=False) - data = out - length = np.atleast_1d(np.sum(data * data, axis)) - np.sqrt(length, length) - if axis is not None: - length = np.expand_dims(length, axis) - data /= length - if out is None: - return data - - -class Object(object): - rootJointType = 'freeflyer' - urdfSuffix = "" - srdfSuffix = "" - - def __init__(self, object_name, pkg_name): - self.__setattr__('urdfName', object_name) - self.__setattr__('packageName', pkg_name) - self.__setattr__('meshPackageName', pkg_name) - - -class Environment(object): - urdfSuffix = "" - srdfSuffix = "" - - def __init__(self, env_name, pkg_name): - self.__setattr__('urdfName', env_name) - self.__setattr__('packageName', pkg_name) - self.__setattr__('meshPackageName', pkg_name) - - -class Model(object): - def __init__(self, name, pkg_name, urdf_name='', srdf_name='', surface='', handle=''): - self._name = name - self._pkg_name = pkg_name - if urdf_name == '': - self._urdf_name = name - else: - self._urdf_name = urdf_name - if srdf_name == '': - self._srdf_name = name - else: - self._srdf_name = srdf_name - self._surface = surface - self._handle = handle - - @property - def name(self): - return self._name - - @property - def pkg_name(self): - return self._pkg_name - - @property - def urdf_name(self): - return self._urdf_name - - @property - def srdf_name(self): - return self._srdf_name - - @property - def surface(self): - return self._surface - - @property - def handle(self): - return self._handle - - -class Gripper(object): - - def __init__(self, robot_model, name, fingers, finger_joints, joint_values): - super(Gripper, self).__init__() - - self._dof = 2 - assert len(fingers) == len(finger_joints) == len(joint_values) == self._dof - - self._fingers = fingers - self._finger_joints = finger_joints - self._joint_values = joint_values - - self._name = name - self._robot_model = robot_model - - @property - def dof(self): - return self._dof - - @property - def name(self): - return self._name - - @property - def fingers(self): - return self._fingers - - @property - def joints(self): - return self._finger_joints - - @property - def values(self): - return self._joint_values - - def get_urdf(self): - pass - - -class HPPManipulationInterface(object): - - def __init__( - self, - env_name, - env_pkg_name, - env_surface, - object_name, - object_pkg_name, - object_surface, - object_handle, - robot_name, - robot_pkg_name, - robot_urdf_name, - robot_srdf_name, - robot_bound, - object_bound, - gripper_name, - fingers, - finger_joints, - finger_joint_values, - **kwargs - ): - super(HPPManipulationInterface, self).__init__() - - loadServerPlugin("corbaserver", "manipulation-corba.so") - Client().problem.resetProblem() - - self._rm = Model(robot_name, robot_pkg_name, robot_urdf_name, robot_srdf_name) - self._om = Model(object_name, object_pkg_name, surface=object_surface, handle=object_handle) - self._em = Model(env_name, env_pkg_name, surface=env_surface) - self._gm = Gripper(self._rm, gripper_name, fingers, finger_joints, finger_joint_values) - - self._work_modes = namedtuple("work_modes", "idle approach grasp") - self._previous_mode = self._work_modes.idle - self._mode = self._work_modes.idle - - self._robot = self._create_robot() - self._problem_solver = ProblemSolver(self._robot) - - self._problem_solver.loadPlugin('manipulation-spline-gradient-based.so') - self._problem_solver.addPathOptimizer('RandomShortcut') - - self._viewer_factory = self._create_viewer_factory() # need to create this no matter if enable_viewer - - self._robot.setJointBounds("{}/root_joint".format(self._rm.name), robot_bound) - self._robot.setJointBounds("{}/root_joint".format(self._om.name), object_bound) - - # An absolute value, if the threshold is surpassed, will raise the error `A configuration has no node` - self._problem_solver.setErrorThreshold(3e-3) - self._problem_solver.setMaxIterProjection(80) - - self._problem_solver.setTimeOutPathPlanning(30) - - rospy.loginfo('Using path planner {}, available planners are: {}'.format( - self._problem_solver.getSelected('PathPlanner'), self._problem_solver.getAvailable('PathPlanner'))) - - self._q_current = self._robot.getCurrentConfig() - self._q_goal = self._q_current[::] - - self._lock_hand = self._create_locks() - - self._graph_id = 0 - self._constraint_graph = self._create_constraint_graph() - - self._object_transform = rotation_matrix(-np.pi / 2, (0, 1, 0)) - - def _create_robot(self, root_joint_type='planar'): - class CompositeRobot(Parent): - urdfFilename = "package://{}/urdf/{}.urdf".format(self._rm.pkg_name, self._rm.urdf_name) - srdfFilename = "package://{}/srdf/{}.srdf".format(self._rm.pkg_name, self._rm.srdf_name) - rootJointType = root_joint_type - - def __init__(self, composite_name, robot_name, load=True, - root_joint="planar", **kwargs): - Parent.__init__(self, composite_name, robot_name, root_joint, load, **kwargs) - - robot = CompositeRobot('{}-{}'.format(self._rm.name, self._om.name), self._rm.name) - return robot - - def _create_viewer_factory(self): - object_to_grasp = Object(self._om.name, self._om.pkg_name) - environment = Environment(self._em.name, self._em.pkg_name) - viewer_factory = ViewerFactory(self._problem_solver) - viewer_factory.loadObjectModel(object_to_grasp, self._om.name) - viewer_factory.loadEnvironmentModel(environment, self._em.name) - return viewer_factory - - def _create_locks(self): - lock_hand = self._gm.fingers - for i in range(self._gm.dof): - self._problem_solver.createLockedJoint( - self._gm.fingers[i], '{}/{}'.format(self._rm.name, self._gm.joints[i]), [self._gm.values[i], ] - ) - return lock_hand - - def _create_constraint_graph(self): - self._graph_id += 1 - graph_name = 'graph_{}'.format(self._graph_id) - constraint_graph = ConstraintGraph(self._robot, graph_name) - rospy.loginfo('Created new constraint graph: {}'.format(graph_name)) - factory = ConstraintGraphFactory(constraint_graph) - factory.setGrippers(["{}/{}".format(self._rm.name, self._gm.name), ]) - factory.environmentContacts(["{}/{}".format(self._em.name, self._em.surface), ]) - factory.setObjects([self._om.name, ], - [["{}/{}".format(self._om.name, self._om.handle), ], ], - [["{}/{}".format(self._om.name, self._om.surface), ], ]) - factory.setRules([Rule([".*"], [".*"], True), ]) - factory.generate() - constraint_graph.addConstraints(graph=True, constraints=Constraints(numConstraints=self._lock_hand)) - constraint_graph.initialize() - return constraint_graph - - def _update_constraints(self): - """Update the constraints if 1) Working mode turned from other modes to the grasp mode. In this mode, - we need to add extra constraints to the edges such that the base do not move during grasping. 2) Working - mode turned from grasping to other modes, where we need to create a new graph since the original constrained - graph cannot be used. When new graph is created, we increase the count of graph_id to prevent naming ambiguous. - - Returns: - None - """ - if self._previous_mode == self._work_modes.grasp and self._mode != self._work_modes.grasp: - self._constraint_graph = self._create_constraint_graph() - if self._previous_mode != self._work_modes.grasp and self._mode == self._work_modes.grasp: - self._problem_solver.createLockedJoint("fix_base", "{}/root_joint".format(self._rm.name), [0, 0, 1, 0]) - for e in self._constraint_graph.edges.keys(): - self._constraint_graph.addConstraints(edge=e, constraints=Constraints(numConstraints=["fix_base"])) - self._constraint_graph.initialize() - self._previous_mode = self._mode - - def _make_plan(self): - self._problem_solver.addGoalConfig(self._q_goal) - - self._problem_solver.setInitialConfig(self._q_current) - - print("Current configuration:\n{}".format(["{0:0.8f}".format(i) for i in self._q_current])) - print("Goal configuration:\n{}".format(["{0:0.8f}".format(i) for i in self._q_goal])) - - self._update_constraints() - try: - time_spent = self._problem_solver.solve() - rospy.loginfo('Plan solved in {}h-{}m-{}s-{}ms'.format(*time_spent)) - except BaseException as e: - rospy.logwarn('Failed to solve due to {}'.format(e)) - self._problem_solver.resetGoalConfigs() - self._mode = self._work_modes.idle - return False - - viewer = self._viewer_factory.createViewer() - viewer(self._q_current) - path_player = PathPlayer(viewer) - while input('Plan finished! Enter q to quit the current path, enter other keys to replay\n') != 'q': - path_player(self._last_path_id) - - self._problem_solver.resetGoalConfigs() - self._mode = self._work_modes.idle - return True - - def make_grasping_plan(self): - """Make a grasping plan. - - Returns: - True if success, False otherwise. - """ - # [0:4] position and orientation of the base in the world frame; - # [4:7] 2 head joints and one torso joint; - # [7:14] left arm joints - # [14:16] left gripper joints - # [16:23] right arm joints - # [23:25] right gripper joints - # [25:32] object pose - # FIXME This works - # self._q_current = [-1.20326676, -0.59723202, -0.99997469, -0.00711415, -0.00000287, 0.00000000, 0.00023268, - # 1.00024558, 0.80147044, -0.49938611, -1.98775687, 0.00035514, 2.50160673, 1.57078452, - # 0.03928489, 0.04000000, -1.00150619, 0.80181510, 0.49849963, -1.98774401, 0.00041852, - # 2.50161359, -0.00001253, 0.04000000, 0.03928675, -2.40000000, -0.60000000, 0.96449543, - # 0.00000000, -0.70710678, 0.00000000, 0.70710678] - # self._q_goal = [-1.20326676, -0.59723202, -0.99997469, -0.00711415, -0.00000287, 0.00000000, 0.00023268, - # 1.00024558, 0.80147044, -0.49938611, -1.98775687, 0.00035514, 2.50160673, 1.57078452, - # 0.03928489, 0.04000000, -1.00150619, 0.80181510, 0.49849963, -1.98774401, 0.00041852, - # 2.50161359, -0.00001253, 0.04000000, 0.03928675, -2.40000000, -0.65000000, 0.96500000, - # 0.00000000, -0.70710678, 0.00000000, 0.70710678] - - # FIXME This does not work - # self._q_current = [-1.20436121, -0.59688047, -0.99999128, -0.00417561, -0.00079096, -0.00000157, 0.00013213, - # 0.99999724, 0.80167330, -0.49945623, -1.98709694, 0.00038990, 2.50165400, 1.57077541, - # 0.03904694, 0.04000000, -1.00156973, 0.80199416, 0.49850983, -1.98703453, 0.00042482, - # 2.50168172, -0.00001558, 0.04000000, 0.03796183, -2.40000000, -0.60000000, 0.96449543, - # 0.00000000, -0.70710678, -0.00000000, 0.70710678] - # self._q_goal = [-1.20000000, -0.60000000, -1.00000000, -0.00000000, -0.00079096, -0.00000157, 0.00013213, - # 0.99999724, 0.80167330, -0.49945623, -1.98709694, 0.00038990, 2.50165400, 1.57077541, - # 0.03904694, 0.04000000, -1.00156973, 0.80199416, 0.49850983, -1.98703453, 0.00042482, - # 2.50168172, -0.00001558, 0.04000000, 0.03796183, -2.40000000, -0.65000000, 0.96500000, - # 0.00000000, -0.70710678, 0.00000000, 0.70710678] - - self._q_current = [-1.20521688, -0.59402480, -0.99999403, -0.00345516, -0.00229608, -0.00000708, 0.00021047, - 0.99907754, 0.80188692, -0.49995588, -1.99145829, 0.00025645, 2.50104214, 1.57078802, - 0.03903948, 0.04000000, -1.00153566, 0.80045072, 0.49898227, -1.99067805, 0.00027125, - 2.50113516, -0.00001007, 0.04000000, 0.03795699, -2.40000000, - -0.60000000, 0.96449543, 0.00000000, -0.70710678, -0.00000000, 0.70710678] - self._q_goal = [-1.20521688, -0.59402480, -0.99999403, -0.00345516, -0.00229608, -0.00000708, 0.00021047, - 0.99907754, 0.80188692, -0.49995588, -1.99145829, 0.00025645, 2.50104214, 1.57078802, - 0.03903948, 0.04000000, -1.00153566, 0.80045072, 0.49898227, -1.99067805, 0.00027125, - 2.50113516, -0.00001007, 0.04000000, 0.03795699, -2.40000000, - -0.65000000, 0.96500000, 0.00000000, -0.70710678, 0.00000000, 0.70710678] - - self._mode = self._work_modes.grasp - return self._make_plan() - - @property - def _last_path_id(self): - return self._problem_solver.numberPaths() - 1 - - -if __name__ == '__main__': - server_process = subprocess.Popen(["hppcorbaserver"], start_new_session=True) - rospy.sleep(5) - viewer_process = subprocess.Popen(["gepetto-gui", "-c", "basic"], - stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, - start_new_session=True) - interface = HPPManipulationInterface('clover_lab', 'clover_lab', 'base_cabinet_worktop_top', - 'cube_30', 'clover_lab', 'cube_30_surface', 'cube_30_handle2', - 'curi', 'curi_description', 'curi_pinocchio', 'curi_pinocchio', - [-2.5, 2.5, -1.5, 1.5], [-3, 3, -1.5, 1.5, 0, 2], 'l_gripper', - ["panda_left_leftfinger", "panda_left_rightfinger"], - ["panda_left_finger_joint1", "panda_left_finger_joint2"], [0.04, 0.04]) - interface.make_grasping_plan() - if server_process is not None: - server_process.kill() - if viewer_process is not None: - viewer_process.kill() diff --git a/src/rotools/hpp/manipulation/minimal_test_2.py b/src/rotools/hpp/manipulation/minimal_test_2.py index 5f7cf4b..f7b7de5 100644 --- a/src/rotools/hpp/manipulation/minimal_test_2.py +++ b/src/rotools/hpp/manipulation/minimal_test_2.py @@ -8,8 +8,14 @@ import rospy from hpp.corbaserver.manipulation.robot import Robot as Parent -from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule, \ - Constraints, ConstraintGraphFactory, Client +from hpp.corbaserver.manipulation import ( + ProblemSolver, + ConstraintGraph, + Rule, + Constraints, + ConstraintGraphFactory, + Client, +) from hpp.corbaserver import loadServerPlugin from hpp.gepetto import PathPlayer @@ -21,15 +27,19 @@ def rotation_matrix(angle, direction, point=None): cosa = math.cos(angle) direction = unit_vector(direction[:3]) # rotation matrix around unit vector - R = np.array(((cosa, 0.0, 0.0), - (0.0, cosa, 0.0), - (0.0, 0.0, cosa)), dtype=np.float64) + R = np.array( + ((cosa, 0.0, 0.0), (0.0, cosa, 0.0), (0.0, 0.0, cosa)), dtype=np.float64 + ) R += np.outer(direction, direction) * (1.0 - cosa) direction *= sina - R += np.array(((0.0, -direction[2], direction[1]), - (direction[2], 0.0, -direction[0]), - (-direction[1], direction[0], 0.0)), - dtype=np.float64) + R += np.array( + ( + (0.0, -direction[2], direction[1]), + (direction[2], 0.0, -direction[0]), + (-direction[1], direction[0], 0.0), + ), + dtype=np.float64, + ) M = np.identity(4) M[:3, :3] = R if point is not None: @@ -59,14 +69,14 @@ def unit_vector(data, axis=None, out=None): class Object(object): - rootJointType = 'freeflyer' + rootJointType = "freeflyer" urdfSuffix = "" srdfSuffix = "" def __init__(self, object_name, pkg_name): - self.__setattr__('urdfName', object_name) - self.__setattr__('packageName', pkg_name) - self.__setattr__('meshPackageName', pkg_name) + self.__setattr__("urdfName", object_name) + self.__setattr__("packageName", pkg_name) + self.__setattr__("meshPackageName", pkg_name) class Environment(object): @@ -74,20 +84,22 @@ class Environment(object): srdfSuffix = "" def __init__(self, env_name, pkg_name): - self.__setattr__('urdfName', env_name) - self.__setattr__('packageName', pkg_name) - self.__setattr__('meshPackageName', pkg_name) + self.__setattr__("urdfName", env_name) + self.__setattr__("packageName", pkg_name) + self.__setattr__("meshPackageName", pkg_name) class Model(object): - def __init__(self, name, pkg_name, urdf_name='', srdf_name='', surface='', handle=''): + def __init__( + self, name, pkg_name, urdf_name="", srdf_name="", surface="", handle="" + ): self._name = name self._pkg_name = pkg_name - if urdf_name == '': + if urdf_name == "": self._urdf_name = name else: self._urdf_name = urdf_name - if srdf_name == '': + if srdf_name == "": self._srdf_name = name else: self._srdf_name = srdf_name @@ -120,7 +132,6 @@ def handle(self): class Gripper(object): - def __init__(self, robot_model, name, fingers, finger_joints, joint_values): super(Gripper, self).__init__() @@ -159,27 +170,26 @@ def get_urdf(self): class HPPManipulationInterface(object): - def __init__( - self, - env_name, - env_pkg_name, - env_surface, - object_name, - object_pkg_name, - object_surface, - object_handle, - robot_name, - robot_pkg_name, - robot_urdf_name, - robot_srdf_name, - robot_bound, - object_bound, - gripper_name, - fingers, - finger_joints, - finger_joint_values, - **kwargs + self, + env_name, + env_pkg_name, + env_surface, + object_name, + object_pkg_name, + object_surface, + object_handle, + robot_name, + robot_pkg_name, + robot_urdf_name, + robot_srdf_name, + robot_bound, + object_bound, + gripper_name, + fingers, + finger_joints, + finger_joint_values, + **kwargs ): super(HPPManipulationInterface, self).__init__() @@ -187,9 +197,13 @@ def __init__( Client().problem.resetProblem() self._rm = Model(robot_name, robot_pkg_name, robot_urdf_name, robot_srdf_name) - self._om = Model(object_name, object_pkg_name, surface=object_surface, handle=object_handle) + self._om = Model( + object_name, object_pkg_name, surface=object_surface, handle=object_handle + ) self._em = Model(env_name, env_pkg_name, surface=env_surface) - self._gm = Gripper(self._rm, gripper_name, fingers, finger_joints, finger_joint_values) + self._gm = Gripper( + self._rm, gripper_name, fingers, finger_joints, finger_joint_values + ) self._work_modes = namedtuple("work_modes", "idle approach grasp") self._previous_mode = self._work_modes.idle @@ -198,22 +212,28 @@ def __init__( self._robot = self._create_robot() self._problem_solver = ProblemSolver(self._robot) - self._problem_solver.loadPlugin('manipulation-spline-gradient-based.so') - self._problem_solver.addPathOptimizer('RandomShortcut') + self._problem_solver.loadPlugin("manipulation-spline-gradient-based.so") + self._problem_solver.addPathOptimizer("RandomShortcut") - self._viewer_factory = self._create_viewer_factory() # need to create this no matter if enable_viewer + self._viewer_factory = ( + self._create_viewer_factory() + ) # need to create this no matter if enable_viewer self._robot.setJointBounds("{}/root_joint".format(self._rm.name), robot_bound) self._robot.setJointBounds("{}/root_joint".format(self._om.name), object_bound) # An absolute value, if the threshold is surpassed, will raise the error `A configuration has no node` - self._problem_solver.setErrorThreshold(3e-3) + self._problem_solver.setErrorThreshold(1e-3) self._problem_solver.setMaxIterProjection(80) self._problem_solver.setTimeOutPathPlanning(30) - print('Using path planner {}, available planners are: {}'.format( - self._problem_solver.getSelected('PathPlanner'), self._problem_solver.getAvailable('PathPlanner'))) + print( + "Using path planner {}, available planners are: {}".format( + self._problem_solver.getSelected("PathPlanner"), + self._problem_solver.getAvailable("PathPlanner"), + ) + ) self._q_init = self._robot.getCurrentConfig() self._q_goal = self._q_init[::] @@ -225,17 +245,31 @@ def __init__( self._object_transform = rotation_matrix(-np.pi / 2, (0, 1, 0)) - def _create_robot(self, root_joint_type='planar'): + def _create_robot(self, root_joint_type="planar"): class CompositeRobot(Parent): - urdfFilename = "package://{}/urdf/{}.urdf".format(self._rm.pkg_name, self._rm.urdf_name) - srdfFilename = "package://{}/srdf/{}.srdf".format(self._rm.pkg_name, self._rm.srdf_name) + urdfFilename = "package://{}/urdf/{}.urdf".format( + self._rm.pkg_name, self._rm.urdf_name + ) + srdfFilename = "package://{}/srdf/{}.srdf".format( + self._rm.pkg_name, self._rm.srdf_name + ) rootJointType = root_joint_type - def __init__(self, composite_name, robot_name, load=True, - root_joint="planar", **kwargs): - Parent.__init__(self, composite_name, robot_name, root_joint, load, **kwargs) - - robot = CompositeRobot('{}-{}'.format(self._rm.name, self._om.name), self._rm.name) + def __init__( + self, + composite_name, + robot_name, + load=True, + root_joint="planar", + **kwargs + ): + Parent.__init__( + self, composite_name, robot_name, root_joint, load, **kwargs + ) + + robot = CompositeRobot( + "{}-{}".format(self._rm.name, self._om.name), self._rm.name + ) return robot def _create_viewer_factory(self): @@ -250,24 +284,54 @@ def _create_locks(self): lock_hand = self._gm.fingers for i in range(self._gm.dof): self._problem_solver.createLockedJoint( - self._gm.fingers[i], '{}/{}'.format(self._rm.name, self._gm.joints[i]), [self._gm.values[i], ] + self._gm.fingers[i], + "{}/{}".format(self._rm.name, self._gm.joints[i]), + [ + self._gm.values[i], + ], ) return lock_hand def _create_constraint_graph(self): self._graph_id += 1 - graph_name = 'graph_{}'.format(self._graph_id) + graph_name = "graph_{}".format(self._graph_id) constraint_graph = ConstraintGraph(self._robot, graph_name) - rospy.loginfo('Created new constraint graph: {}'.format(graph_name)) + rospy.loginfo("Created new constraint graph: {}".format(graph_name)) factory = ConstraintGraphFactory(constraint_graph) - factory.setGrippers(["{}/{}".format(self._rm.name, self._gm.name), ]) - factory.environmentContacts(["{}/{}".format(self._em.name, self._em.surface), ]) - factory.setObjects([self._om.name, ], - [["{}/{}".format(self._om.name, self._om.handle), ], ], - [["{}/{}".format(self._om.name, self._om.surface), ], ]) - factory.setRules([Rule([".*"], [".*"], True), ]) + factory.setGrippers( + [ + "{}/{}".format(self._rm.name, self._gm.name), + ] + ) + factory.environmentContacts( + [ + "{}/{}".format(self._em.name, self._em.surface), + ] + ) + factory.setObjects( + [ + self._om.name, + ], + [ + [ + "{}/{}".format(self._om.name, self._om.handle), + ], + ], + [ + [ + "{}/{}".format(self._om.name, self._om.surface), + ], + ], + ) + factory.setRules( + [ + Rule([".*"], [".*"], True), + ] + ) factory.generate() - constraint_graph.addConstraints(graph=True, constraints=Constraints(numConstraints=self._lock_hand)) + constraint_graph.addConstraints( + graph=True, constraints=Constraints(numConstraints=self._lock_hand) + ) constraint_graph.initialize() return constraint_graph @@ -275,17 +339,28 @@ def _make_plan(self): self._problem_solver.addGoalConfig(self._q_goal) self._problem_solver.setInitialConfig(self._q_init) - print("Current configuration:\n{}".format(["{0:0.8f}".format(i) for i in self._q_init])) - print("Goal configuration:\n{}".format(["{0:0.8f}".format(i) for i in self._q_goal])) + print( + "Current configuration:\n{}".format( + ["{0:0.8f}".format(i) for i in self._q_init] + ) + ) + print( + "Goal configuration:\n{}".format( + ["{0:0.8f}".format(i) for i in self._q_goal] + ) + ) try: time_spent = self._problem_solver.solve() - print('Plan solved in {}h-{}m-{}s-{}ms'.format(*time_spent)) + print("Plan solved in {}h-{}m-{}s-{}ms".format(*time_spent)) except BaseException as e: print(self._constraint_graph.nodes) - print('Failed to solve due to {}'.format(e)) - res, q, err = self._constraint_graph.applyNodeConstraints('free', self._q_init) - print(res, err, '\n', q) + print("Failed to solve due to {}".format(e)) + res, q, err = self._constraint_graph.applyNodeConstraints( + "free", self._q_init + ) + print(res, err, "\n", q) + print(self._constraint_graph.getNode(self._q_init)) self._problem_solver.resetGoalConfigs() self._mode = self._work_modes.idle return False @@ -293,7 +368,12 @@ def _make_plan(self): viewer = self._viewer_factory.createViewer() viewer(self._q_init) path_player = PathPlayer(viewer) - while input('Plan finished! Enter q to quit the current path, enter other keys to replay\n') != 'q': + while ( + input( + "Plan finished! Enter q to quit the current path, enter other keys to replay\n" + ) + != "q" + ): path_player(self._last_path_id) self._problem_solver.resetGoalConfigs() @@ -326,19 +406,74 @@ def make_test_plan(self): # 0.00000000, -0.70710678, 0.00000000, 0.70710678] # FIXME This does not work - self._q_init = [0.6828121022644879634, 0.0703259613989929006, 0.8687425541219662151, 0.4952639444353310050, - 0.0001423814094132478, 0.0000010956036508039, 0.0003569450372355056, 1.0006461567295650283, - 0.8029290429009514618, -0.4976525619349045360, -1.9705628949627718161, 0.0007762117476604057, - 2.5024607950674822021, 1.5698918488567741303, 0.0366880180611687187, 0.0400000000000000008, - -1.0032131005843512028, 0.8043047023383176342, 0.4956911330734958265, -1.9707136657764974252, - 0.0008677962229538790, 2.5024503019683517202, -0.0000280178770064655, 0.0400000000000000008, - 0.0355720185971239375, -2.3999999994155452043, -0.5999999998769479648, 0.9644954319622398575, - 0.0000000000000003189, -0.7071067811865474617, -0.0000000000000003182, 0.7071067811865474617] - self._q_goal = [0.97013911, 0.21839769, 0.70710678, 0.70710678, 0.00011383, 0.00000100, 0.00027770, 1.00063293, - 0.80204085, -0.49794376, -1.97783062, 0.00056355, 2.50151300, 1.56990079, 0.03734613, - 0.04000000, -1.00249611, 0.80319770, 0.49647833, - -1.97797381, 0.00062338, 2.50150087, -0.00001964, 0.04000000, 0.03622407, -2.40000000, - -0.60000000, 0.96449543, 0.00000000, -0.70710678, -0.00000000, 0.70710678] + self._q_init = [ + 0.6828121022644879634, + 0.0703259613989929006, + 0.8687425541219662151, + 0.4952639444353310050, + 0.0001423814094132478, + 0.0000010956036508039, + 0.0003569450372355056, + 1.0006461567295650283, + 0.8029290429009514618, + -0.4976525619349045360, + -1.9705628949627718161, + 0.0007762117476604057, + 2.5024607950674822021, + 1.5698918488567741303, + 0.04, + 0.04, + -1.0032131005843512028, + 0.8043047023383176342, + 0.4956911330734958265, + -1.9707136657764974252, + 0.0008677962229538790, + 2.5024503019683517202, + -0.0000280178770064655, + 0.04, + 0.04, + -2.3999999994155452043, + -0.5999999998769479648, + 0.9644954319622398575, + 0.0000000000000003189, + -0.7071067811865474617, + -0.0000000000000003182, + 0.7071067811865474617, + ] + self._q_goal = [ + 0.97013911, + 0.21839769, + 0.70710678, + 0.70710678, + 0.00011383, + 0.00000100, + 0.00027770, + 1.00063293, + 0.80204085, + -0.49794376, + -1.97783062, + 0.00056355, + 2.50151300, + 1.56990079, + 0.03734613, + 0.04000000, + -1.00249611, + 0.80319770, + 0.49647833, + -1.97797381, + 0.00062338, + 2.50150087, + -0.00001964, + 0.04000000, + 0.03622407, + -2.40000000, + -0.60000000, + 0.96449543, + 0.00000000, + -0.70710678, + -0.00000000, + 0.70710678, + ] self._mode = self._work_modes.approach return self._make_plan() @@ -348,19 +483,35 @@ def _last_path_id(self): return self._problem_solver.numberPaths() - 1 -if __name__ == '__main__': +if __name__ == "__main__": server_process = subprocess.Popen(["hppcorbaserver"], start_new_session=True) rospy.sleep(5) - viewer_process = subprocess.Popen(["gepetto-gui", "-c", "basic"], - stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, - start_new_session=True) + viewer_process = subprocess.Popen( + ["gepetto-gui", "-c", "basic"], + stdout=subprocess.DEVNULL, + stderr=subprocess.DEVNULL, + start_new_session=True, + ) try: - interface = HPPManipulationInterface('clover_lab', 'clover_lab', 'base_cabinet_worktop_top', - 'cube_30', 'clover_lab', 'cube_30_surface', 'cube_30_handle2', - 'curi', 'curi_description', 'curi_pinocchio', 'curi_pinocchio', - [-2.5, 2.5, -1.5, 1.5], [-3, 3, -1.5, 1.5, 0, 2], 'l_gripper', - ["panda_left_leftfinger", "panda_left_rightfinger"], - ["panda_left_finger_joint1", "panda_left_finger_joint2"], [0.04, 0.04]) + interface = HPPManipulationInterface( + "clover_lab", + "clover_lab", + "base_cabinet_worktop_top", + "cube_30", + "clover_lab", + "cube_30_surface", + "cube_30_handle2", + "curi", + "curi_description", + "curi_pinocchio", + "curi_pinocchio", + [-2.5, 2.5, -1.5, 1.5], + [-3, 3, -1.5, 1.5, 0, 2], + "l_gripper", + ["panda_left_leftfinger", "panda_left_rightfinger"], + ["panda_left_finger_joint1", "panda_left_finger_joint2"], + [0.04, 0.04], + ) interface.make_test_plan() except BaseException as e: print(e) diff --git a/src/rotools/hpp/manipulation/pr2_interface.py b/src/rotools/hpp/manipulation/pr2_interface.py index ea34dbc..e2db8a2 100644 --- a/src/rotools/hpp/manipulation/pr2_interface.py +++ b/src/rotools/hpp/manipulation/pr2_interface.py @@ -6,8 +6,14 @@ import rospy from hpp.corbaserver.manipulation.robot import Robot as Parent -from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, Rule, \ - Constraints, ConstraintGraphFactory, Client +from hpp.corbaserver.manipulation import ( + ProblemSolver, + ConstraintGraph, + Rule, + Constraints, + ConstraintGraphFactory, + Client, +) from hpp.corbaserver import loadServerPlugin from hpp.gepetto import PathPlayer @@ -22,30 +28,29 @@ class HPPManipulationInterface(object): - def __init__( - self, - env_name, - env_pkg_name, - env_surface, - object_name, - object_pkg_name, - object_surface, - object_handle, - robot_name, - robot_pkg_name, - robot_urdf_name, - robot_srdf_name, - robot_bound, - object_bound, - gripper_name, - fingers, - finger_joints, - finger_joint_values, - joint_cmd_topic, - base_cmd_topic, - enable_viewer=True, - **kwargs + self, + env_name, + env_pkg_name, + env_surface, + object_name, + object_pkg_name, + object_surface, + object_handle, + robot_name, + robot_pkg_name, + robot_urdf_name, + robot_srdf_name, + robot_bound, + object_bound, + gripper_name, + fingers, + finger_joints, + finger_joint_values, + joint_cmd_topic, + base_cmd_topic, + enable_viewer=True, + **kwargs ): super(HPPManipulationInterface, self).__init__() @@ -53,9 +58,13 @@ def __init__( Client().problem.resetProblem() self._rm = Model(robot_name, robot_pkg_name, robot_urdf_name, robot_srdf_name) - self._om = Model(object_name, object_pkg_name, surface=object_surface, handle=object_handle) + self._om = Model( + object_name, object_pkg_name, surface=object_surface, handle=object_handle + ) self._em = Model(env_name, env_pkg_name, surface=env_surface) - self._gm = Gripper(self._rm, gripper_name, fingers, finger_joints, finger_joint_values) + self._gm = Gripper( + self._rm, gripper_name, fingers, finger_joints, finger_joint_values + ) self._robot = self._create_robot() self._problem_solver = ProblemSolver(self._robot) @@ -77,29 +86,39 @@ def __init__( self._q_current = self._robot.getCurrentConfig() self._q_current[0:2] = [-3.2, -4] - rank = self._robot.rankInConfiguration['{}/{}'.format(self._rm.name, self._gm.joints[0])] + rank = self._robot.rankInConfiguration[ + "{}/{}".format(self._rm.name, self._gm.joints[0]) + ] self._q_current[rank] = 0.5 - rank = self._robot.rankInConfiguration['{}/{}'.format(self._rm.name, self._gm.joints[1])] + rank = self._robot.rankInConfiguration[ + "{}/{}".format(self._rm.name, self._gm.joints[1]) + ] self._q_current[rank] = 0.5 - rank = self._robot.rankInConfiguration['{}/torso_lift_joint'.format(self._rm.name)] + rank = self._robot.rankInConfiguration[ + "{}/torso_lift_joint".format(self._rm.name) + ] self._q_current[rank] = 0.2 - rank = self._robot.rankInConfiguration['{}/root_joint'.format(self._om.name)] - self._q_current[rank:rank + 3] = [-2.5, -4, 0.8] + rank = self._robot.rankInConfiguration["{}/root_joint".format(self._om.name)] + self._q_current[rank : rank + 3] = [-2.5, -4, 0.8] self._q_goal = self._q_current[::] self._q_goal[0:2] = [-3.2, -4] - rank = self._robot.rankInConfiguration['{}/root_joint'.format(self._om.name)] - self._q_goal[rank:rank + 3] = [-3.5, -4, 0.8] + rank = self._robot.rankInConfiguration["{}/root_joint".format(self._om.name)] + self._q_goal[rank : rank + 3] = [-3.5, -4, 0.8] self._lock_hand = self._create_lock_hand() self._constrain_graph = self._create_constrain_graph() - def _create_robot(self, root_joint_type='planar'): + def _create_robot(self, root_joint_type="planar"): class CompositeRobot(Parent): - urdfFilename = "package://{}/urdf/{}.urdf".format(self._rm.pkg_name, self._rm.urdf_name) - srdfFilename = "package://{}/srdf/{}.srdf".format(self._rm.pkg_name, self._rm.srdf_name) + urdfFilename = "package://{}/urdf/{}.urdf".format( + self._rm.pkg_name, self._rm.urdf_name + ) + srdfFilename = "package://{}/srdf/{}.srdf".format( + self._rm.pkg_name, self._rm.srdf_name + ) rootJointType = root_joint_type # \param compositeName name of the composite robot that will be built later, @@ -109,11 +128,21 @@ class CompositeRobot(Parent): # problem. # \param rootJointType type of root joint among ("freeflyer", "planar", # "anchor"), - def __init__(self, compositeName, robotName, load=True, - rootJointType="planar", **kwargs): - Parent.__init__(self, compositeName, robotName, rootJointType, load, **kwargs) - - robot = CompositeRobot('{}-{}'.format(self._rm.name, self._om.name), self._rm.name) + def __init__( + self, + compositeName, + robotName, + load=True, + rootJointType="planar", + **kwargs + ): + Parent.__init__( + self, compositeName, robotName, rootJointType, load, **kwargs + ) + + robot = CompositeRobot( + "{}-{}".format(self._rm.name, self._om.name), self._rm.name + ) return robot def _create_viewer_factory(self): @@ -128,21 +157,51 @@ def _create_lock_hand(self): lock_hand = self._gm.fingers for i in range(self._gm.dof): self._problem_solver.createLockedJoint( - self._gm.fingers[i], '{}/{}'.format(self._rm.name, self._gm.joints[i]), [self._gm.values[i], ] + self._gm.fingers[i], + "{}/{}".format(self._rm.name, self._gm.joints[i]), + [ + self._gm.values[i], + ], ) return lock_hand def _create_constrain_graph(self): - constrain_graph = ConstraintGraph(self._robot, 'graph') + constrain_graph = ConstraintGraph(self._robot, "graph") factory = ConstraintGraphFactory(constrain_graph) - factory.setGrippers(["{}/{}".format(self._rm.name, self._gm.name), ]) - factory.environmentContacts(["{}/{}".format(self._em.name, self._em.surface), ]) - factory.setObjects([self._om.name, ], - [["{}/{}".format(self._om.name, self._om.handle), ], ], - [["{}/{}".format(self._om.name, self._om.surface), ], ]) - factory.setRules([Rule([".*"], [".*"], True), ]) + factory.setGrippers( + [ + "{}/{}".format(self._rm.name, self._gm.name), + ] + ) + factory.environmentContacts( + [ + "{}/{}".format(self._em.name, self._em.surface), + ] + ) + factory.setObjects( + [ + self._om.name, + ], + [ + [ + "{}/{}".format(self._om.name, self._om.handle), + ], + ], + [ + [ + "{}/{}".format(self._om.name, self._om.surface), + ], + ], + ) + factory.setRules( + [ + Rule([".*"], [".*"], True), + ] + ) factory.generate() - constrain_graph.addConstraints(graph=True, constraints=Constraints(lockedJoints=self._lock_hand)) + constrain_graph.addConstraints( + graph=True, constraints=Constraints(lockedJoints=self._lock_hand) + ) constrain_graph.initialize() return constrain_graph @@ -150,7 +209,9 @@ def update_current_config(self, msg): if isinstance(msg, JointState): for i, joint_name in enumerate(msg.name): try: - rank = self._robot.rankInConfiguration['{}/{}'.format(self._robot_name, joint_name)] + rank = self._robot.rankInConfiguration[ + "{}/{}".format(self._robot_name, joint_name) + ] self._q_current[rank] = msg.position except KeyError: continue @@ -159,7 +220,9 @@ def update_current_config(self, msg): elif isinstance(msg, Pose): self._set_object_config(self._q_current, msg) else: - rospy.logerr("Msg is not of type JointState/Odometry/Pose: {}".format(type(msg))) + rospy.logerr( + "Msg is not of type JointState/Odometry/Pose: {}".format(type(msg)) + ) def set_object_goal_config(self, object_pose): assert isinstance(object_pose, Pose) @@ -169,8 +232,12 @@ def get_current_base_global_pose(self): return self._get_robot_base_pose(self._q_current) def make_plan(self, pos_tol, ori_tol): - res, q_init_proj, err = self._constrain_graph.applyNodeConstraints("free", self._q_current) - res, q_goal_proj, err = self._constrain_graph.applyNodeConstraints("free", self._q_goal) + res, q_init_proj, err = self._constrain_graph.applyNodeConstraints( + "free", self._q_current + ) + res, q_goal_proj, err = self._constrain_graph.applyNodeConstraints( + "free", self._q_goal + ) self._problem_solver.setInitialConfig(q_init_proj) self._problem_solver.addGoalConfig(q_goal_proj) @@ -178,7 +245,11 @@ def make_plan(self, pos_tol, ori_tol): self._problem_solver.setTargetState( self._constrain_graph.nodes[ - "{}/{} grasps {}/{}".format(self._rm.name, self._gm.name, self._om.name, self._om.handle)]) + "{}/{} grasps {}/{}".format( + self._rm.name, self._gm.name, self._om.name, self._om.handle + ) + ] + ) self._problem_solver.solve() viewer = self._viewer_factory.createViewer() @@ -204,13 +275,23 @@ def _set_robot_base_config(config, base_odom): assert isinstance(base_odom, Odometry) base_pose = base_odom.pose.pose config[0:2] = [base_pose.position.x, base_pose.position.y] - yaw = transform.euler_from_matrix(common.sd_orientation(base_pose.orientation))[-1] + yaw = transform.euler_from_matrix(common.sd_orientation(base_pose.orientation))[ + -1 + ] config[2:4] = [math.cos(yaw), math.sin(yaw)] def _set_object_config(self, config, object_pose): assert isinstance(object_pose, Pose) - rank = self._robot.rankInConfiguration['{}/root_joint'.format(self._object_name)] + rank = self._robot.rankInConfiguration[ + "{}/root_joint".format(self._object_name) + ] # TODO check the order is wxyz - config[rank: rank + 7] = [object_pose.position.x, object_pose.position.y, object_pose.position.z, - object_pose.orientation.w, object_pose.orientation.x, object_pose.orientation.y, - object_pose.orientation.z] + config[rank : rank + 7] = [ + object_pose.position.x, + object_pose.position.y, + object_pose.position.z, + object_pose.orientation.w, + object_pose.orientation.x, + object_pose.orientation.y, + object_pose.orientation.z, + ] diff --git a/src/rotools/moveit/core/interface.py b/src/rotools/moveit/core/interface.py index ec9ff0e..3b78961 100644 --- a/src/rotools/moveit/core/interface.py +++ b/src/rotools/moveit/core/interface.py @@ -8,30 +8,32 @@ import rospy import tf2_ros import tf2_geometry_msgs # import this is mandatory to use PoseStamped msg - import moveit_commander - import geometry_msgs.msg as GeometryMsg - import moveit_msgs.msg as MoveItMsg - import control_msgs.msg as ControlMsg - import trajectory_msgs.msg as TrajectoryMsg - import std_msgs.msg as StdMsg - import sensor_msgs.msg as SensorMsg + import geometry_msgs.msg as geo_msg + import moveit_msgs.msg as moveit_msg + import control_msgs.msg as ctrl_msg + import trajectory_msgs.msg as traj_msg + import std_msgs.msg as std_msg + import sensor_msgs.msg as sensor_msg except ImportError: - pass + rospy = None + tf2_ros = None + moveit_commander = None + geo_msg = None + moveit_msg = None from rotools.utility import common, transform class MoveGroupInterface(object): - def __init__( - self, - robot_description, - ns, - group_names, - ref_frames=None, - ee_links=None, + self, + robot_description, + ns, + group_names, + ref_frames=None, + ee_links=None, ): super(MoveGroupInterface, self).__init__() @@ -39,7 +41,11 @@ def __init__( self.commander = moveit_commander.RobotCommander(robot_description, ns) if not isinstance(group_names, list) and not isinstance(group_names, tuple): - raise TypeError('group_names should be list or tuple, but got {}'.format(type(group_names))) + raise TypeError( + "group_names should be list or tuple, but got {}".format( + type(group_names) + ) + ) self.group_names = group_names self.group_num = len(self.group_names) @@ -48,7 +54,7 @@ def __init__( # Get a set G of all groups of the robot, the used groups G' could be a subset of G all_group_names = self.commander.get_group_names() for name in self.group_names: - assert name in all_group_names, 'Group name {} does not exist'.format(name) + assert name in all_group_names, "Group name {} does not exist".format(name) self.move_groups = [] for name in self.group_names: @@ -105,7 +111,11 @@ def _get_group_by_name(self, group_name): for i, name in enumerate(self.group_names): if name == group_name: return self.move_groups[i] - raise IndexError('The group name {} is not in the known names {}'.format(group_name, self.group_names)) + raise IndexError( + "The group name {} is not in the known names {}".format( + group_name, self.group_names + ) + ) def get_active_joint_names_of_all_groups(self): ret = [] @@ -127,10 +137,10 @@ def get_joint_states_of_all_groups(self): ret.append(group.get_current_joint_values()) return ret - def get_joint_states_of_group(self, group_name, result_type='rad'): + def get_joint_states_of_group(self, group_name, result_type="rad"): group = self._get_group_by_name(group_name) j_values = group.get_current_joint_values() - if result_type == 'deg' or result_type == 'd': + if result_type == "deg" or result_type == "d": j_values = np.rad2deg(j_values) return j_values @@ -159,15 +169,15 @@ def get_frame_of_group(self, group_name): @staticmethod def get_prepare_pose(pose, is_absolute, shift): - """Given a target pose of the end-effector, get the prepare pose + """Given a target pose of the end-effector, get the prepare-pose for the robot to make the eef move to the target pose. :param pose: target pose of the end-effector in base reference frame :param is_absolute: if true, the shift vector is wrt the base frame, else wrt the target pose frame :param shift: shift vector pointing from the target pose to prepare pose, in meters """ - assert isinstance(pose, GeometryMsg.Pose), print("Pose format error") - assert isinstance(shift, GeometryMsg.Point), print("Shift format error") + assert isinstance(pose, geo_msg.Pose), print("Pose format error") + assert isinstance(shift, geo_msg.Point), print("Shift format error") sd_shift = np.array([shift.x, shift.y, shift.z]) if not is_absolute: q = pose.orientation @@ -176,8 +186,8 @@ def get_prepare_pose(pose, is_absolute, shift): return True, common.offset_ros_pose(pose, sd_shift) def get_transformed_pose(self, pose, source_frame, target_frame): - assert isinstance(pose, GeometryMsg.Pose), print("Pose format error") - pose_stamped = GeometryMsg.PoseStamped() + assert isinstance(pose, geo_msg.Pose), print("Pose format error") + pose_stamped = geo_msg.PoseStamped() pose_stamped.pose = pose # pose_stamped.header.stamp = rospy.Time.now() pose_stamped.header.frame_id = source_frame @@ -185,7 +195,11 @@ def get_transformed_pose(self, pose, source_frame, target_frame): transformed_pose = self._tf_buffer.transform(pose_stamped, target_frame) print(transformed_pose.pose) return True, transformed_pose.pose - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + except ( + tf2_ros.LookupException, + tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException, + ): rospy.logerr("Get transformed pose failed to do the transform") return False, None @@ -247,7 +261,7 @@ def all_go_to_joint_states(self, goals): group.stop() return True - def _execute_group_pose(self, group_name, goal, tolerance=0.01, constraint=''): + def _execute_group_pose(self, group_name, goal, tolerance=0.01, constraint=""): """Make the planning group's eef move to given goal pose. :param group_name: str Planning group name @@ -274,7 +288,9 @@ def _execute_group_pose(self, group_name, goal, tolerance=0.01, constraint=''): group.clear_path_constraints() return self._wait_pose_goal_execution(group_name, goal, tolerance) - def _relative_pose_to_absolute_pose(self, group_name, relative_pose, init_pose=None): + def _relative_pose_to_absolute_pose( + self, group_name, relative_pose, init_pose=None + ): """Convert a relative pose in local base frame to global base frame. :param group_name: str Planning group name @@ -294,7 +310,9 @@ def _relative_pose_to_absolute_pose(self, group_name, relative_pose, init_pose=N eef_local_mat = np.dot(np.linalg.inv(local_base_mat), current_pose_mat) # T_le relative_pose_mat = common.sd_pose(relative_pose) # T_ll' # T_be' = T_bl * T_ll' * T_l'e, note that T_l'e == T_le - absolute_pose_mat = np.dot(np.dot(local_base_mat, relative_pose_mat), eef_local_mat) + absolute_pose_mat = np.dot( + np.dot(local_base_mat, relative_pose_mat), eef_local_mat + ) return common.to_ros_pose(absolute_pose_mat) def _eef_pose_to_absolute_pose(self, group_name, relative_pose, init_pose=None): @@ -311,10 +329,12 @@ def _eef_pose_to_absolute_pose(self, group_name, relative_pose, init_pose=None): current_pose = init_pose current_pose_mat = common.sd_pose(current_pose) relative_pose_mat = common.sd_pose(relative_pose) - absolute_pose_mat = np.dot(current_pose_mat, relative_pose_mat) # T_b1 * T_12 = T_b2 + absolute_pose_mat = np.dot( + current_pose_mat, relative_pose_mat + ) # T_b1 * T_12 = T_b2 return common.to_ros_pose(absolute_pose_mat) - def group_goto_pose_goal_abs(self, group_name, goal, tolerance=0.01, constraint=''): + def group_goto_pose_goal_abs(self, group_name, goal, tolerance=0.01, constraint=""): """Move group to the goal pose wrt the global base frame :param group_name: Controlled group name @@ -323,16 +343,18 @@ def group_goto_pose_goal_abs(self, group_name, goal, tolerance=0.01, constraint= :param constraint: str, path constraint. :return: whether goal reached """ - if isinstance(goal, GeometryMsg.PoseStamped): + if isinstance(goal, geo_msg.PoseStamped): goal_pose = goal.pose - elif isinstance(goal, GeometryMsg.Pose): + elif isinstance(goal, geo_msg.Pose): goal_pose = goal else: - raise NotImplementedError('Goal of type {} is not defined'.format(type(goal))) + raise NotImplementedError( + "Goal of type {} is not defined".format(type(goal)) + ) return self._execute_group_pose(group_name, goal_pose, tolerance, constraint) - def group_goto_pose_goal_rel(self, group_name, goal, tolerance=0.01, constraint=''): + def group_goto_pose_goal_rel(self, group_name, goal, tolerance=0.01, constraint=""): """Move group to the goal pose wrt the local base frame :param group_name: Controlled group name @@ -341,17 +363,19 @@ def group_goto_pose_goal_rel(self, group_name, goal, tolerance=0.01, constraint= :param constraint: str, path constraint. :return: whether goal reached """ - if isinstance(goal, GeometryMsg.PoseStamped): + if isinstance(goal, geo_msg.PoseStamped): goal_pose = goal.pose - elif isinstance(goal, GeometryMsg.Pose): + elif isinstance(goal, geo_msg.Pose): goal_pose = goal else: - raise NotImplementedError('Goal of type {} is not defined'.format(type(goal))) + raise NotImplementedError( + "Goal of type {} is not defined".format(type(goal)) + ) abs_goal = self._relative_pose_to_absolute_pose(group_name, goal_pose) return self._execute_group_pose(group_name, abs_goal, tolerance, constraint) - def group_goto_pose_goal_eef(self, group_name, goal, tolerance=0.01, constraint=''): + def group_goto_pose_goal_eef(self, group_name, goal, tolerance=0.01, constraint=""): """Move group to the goal pose wrt the eef frame :param group_name: str Planning group name. @@ -360,9 +384,9 @@ def group_goto_pose_goal_eef(self, group_name, goal, tolerance=0.01, constraint= :param constraint: str, path constraint. :return: """ - if isinstance(goal, GeometryMsg.PoseStamped): + if isinstance(goal, geo_msg.PoseStamped): goal_pose = goal.pose - elif isinstance(goal, GeometryMsg.Pose): + elif isinstance(goal, geo_msg.Pose): goal_pose = goal else: raise NotImplementedError @@ -428,7 +452,9 @@ def _to_absolute_position(self, group_name, relative_position): current_pose_mat = common.sd_pose(current_pose) relative_pose_mat = transform.identity_matrix() relative_pose_mat[0:3, 3] = common.sd_position(relative_position) - absolute_pose_mat = np.dot(current_pose_mat, relative_pose_mat) # T_b1 * T_12 = T_b2 + absolute_pose_mat = np.dot( + current_pose_mat, relative_pose_mat + ) # T_b1 * T_12 = T_b2 absolute_position = absolute_pose_mat[0:3, 3] return absolute_position @@ -451,7 +477,7 @@ def group_shift(self, group_name, axis, goal): """ group = self._get_group_by_name(group_name) # 0,1,2,3,4,5 for x y z roll pitch yaw - axis_list = ['x', 'y', 'z', 'roll', 'pitch', 'yaw'] + axis_list = ["x", "y", "z", "roll", "pitch", "yaw"] axis_id = -1 for i, a in enumerate(axis_list): if a == axis: @@ -495,12 +521,16 @@ def _update_plan_time_stamps(self, group, plan, stamp): velocity_scale = original_stamp / stamp acceleration_scale = velocity_scale curr_state = self.commander.get_current_state() - updated_plan = group.retime_trajectory(curr_state, plan, velocity_scale, acceleration_scale) + updated_plan = group.retime_trajectory( + curr_state, plan, velocity_scale, acceleration_scale + ) # for i in range(points_num): # plan.joint_trajectory.points[i].time_from_start = rospy.Duration.from_sec(t * (i + 1)) return updated_plan - def _build_cartesian_path_for_group(self, group_name, poses, stamp=None, allow_collisions=True): + def _build_cartesian_path_for_group( + self, group_name, poses, stamp=None, allow_collisions=True + ): """Given waypoints in a list of geometry_msgs.Pose, plan a Cartesian path that goes through all waypoints. @@ -510,13 +540,17 @@ def _build_cartesian_path_for_group(self, group_name, poses, stamp=None, allow_c :param allow_collisions: If true, allow collision along the path """ group = self._get_group_by_name(group_name) - if isinstance(poses, GeometryMsg.PoseArray): + if isinstance(poses, geo_msg.PoseArray): poses = poses.poses - plan, fraction = group.compute_cartesian_path(poses, eef_step=0.01, jump_threshold=0, - avoid_collisions=not allow_collisions) + plan, fraction = group.compute_cartesian_path( + poses, + eef_step=0.01, + jump_threshold=0, + avoid_collisions=not allow_collisions, + ) # move_group_interface.h L754 if fraction < 0: - rospy.logwarn('Path fraction less than 0.') + rospy.logwarn("Path fraction less than 0.") if stamp: plan = self._update_plan_time_stamps(group, plan, stamp) return plan @@ -571,8 +605,10 @@ def _execute_group_plan(self, group_name, plan, wait=True): # return False # return True - def all_goto_pose_goals_abs(self, group_names, goals, tolerance, stamps=None, allow_collision=True): - if isinstance(goals, GeometryMsg.PoseArray): + def all_goto_pose_goals_abs( + self, group_names, goals, tolerance, stamps=None, allow_collision=True + ): + if isinstance(goals, geo_msg.PoseArray): goals = goals.poses assert len(goals) == self.group_num for i, goal in enumerate(goals): @@ -581,13 +617,17 @@ def all_goto_pose_goals_abs(self, group_names, goals, tolerance, stamps=None, al stamp = stamps[i] if stamps else None except IndexError: stamp = None - plan = self._build_cartesian_path_for_group(group_name, [goal], stamp, allow_collision) + plan = self._build_cartesian_path_for_group( + group_name, [goal], stamp, allow_collision + ) if not self._execute_group_plan(group_name, plan): return False return True - def all_goto_pose_goals_rel(self, group_names, goals, tolerance, stamps=None, allow_collision=True): - if isinstance(goals, GeometryMsg.PoseArray): + def all_goto_pose_goals_rel( + self, group_names, goals, tolerance, stamps=None, allow_collision=True + ): + if isinstance(goals, geo_msg.PoseArray): goals = goals.poses assert len(goals) == self.group_num for i, goal in enumerate(goals): @@ -597,13 +637,17 @@ def all_goto_pose_goals_rel(self, group_names, goals, tolerance, stamps=None, al except IndexError: stamp = None goal = self._relative_pose_to_absolute_pose(group_name, goal) - plan = self._build_cartesian_path_for_group(group_name, [goal], stamp, allow_collision) + plan = self._build_cartesian_path_for_group( + group_name, [goal], stamp, allow_collision + ) if not self._execute_group_plan(group_name, plan, wait=True): return False return True - def all_goto_pose_goals_eef(self, group_names, goals, tolerance, stamps=None, allow_collision=True): - if isinstance(goals, GeometryMsg.PoseArray): + def all_goto_pose_goals_eef( + self, group_names, goals, tolerance, stamps=None, allow_collision=True + ): + if isinstance(goals, geo_msg.PoseArray): goals = goals.poses assert len(goals) == self.group_num for i, goal in enumerate(goals): @@ -613,27 +657,33 @@ def all_goto_pose_goals_eef(self, group_names, goals, tolerance, stamps=None, al except IndexError: stamp = None goal = self._eef_pose_to_absolute_pose(group_name, goal) - plan = self._build_cartesian_path_for_group(group_name, [goal], stamp, allow_collision) + plan = self._build_cartesian_path_for_group( + group_name, [goal], stamp, allow_collision + ) if not self._execute_group_plan(group_name, plan, wait=True): return False return True - def add_box(self, group_name, box_name, box_pose, box_size, is_absolute, auto_suffix=False): + def add_box( + self, group_name, box_name, box_pose, box_size, is_absolute, auto_suffix=False + ): group = self._get_group_by_name(group_name) - box_pose_stamped = GeometryMsg.PoseStamped() + box_pose_stamped = geo_msg.PoseStamped() box_pose_stamped.pose = box_pose if is_absolute: box_pose_stamped.header.frame_id = group.get_planning_frame() else: box_pose_stamped.header.frame_id = group.get_end_effector_link() - if box_name == '': - box_name = 'box' + if box_name == "": + box_name = "box" if auto_suffix: box_name += str(self._obj_suffix) self._obj_suffix += 1 - assert isinstance(box_size, GeometryMsg.Point) + assert isinstance(box_size, geo_msg.Point) # size must be iterable - self.scene.add_box(box_name, box_pose_stamped, size=(box_size.x, box_size.y, box_size.z)) + self.scene.add_box( + box_name, box_pose_stamped, size=(box_size.x, box_size.y, box_size.z) + ) start = rospy.get_time() seconds = rospy.get_time() @@ -645,13 +695,22 @@ def add_box(self, group_name, box_name, box_pose, box_size, is_absolute, auto_su return False def attach_box(self, group_name, eef_group_name, box_name, box_pose, box_size): - ok = self.add_box(group_name, box_name, box_pose, box_size, is_absolute=True, auto_suffix=False) + ok = self.add_box( + group_name, + box_name, + box_pose, + box_size, + is_absolute=True, + auto_suffix=False, + ) if not ok: return False group = self._get_group_by_name(group_name) grasping_group = eef_group_name touch_links = self.commander.get_link_names(group=grasping_group) - self.scene.attach_box(group.get_end_effector_link(), box_name, touch_links=touch_links) + self.scene.attach_box( + group.get_end_effector_link(), box_name, touch_links=touch_links + ) return True def detach_object(self, group_name, obj_name): @@ -668,21 +727,27 @@ def remove_object(self, obj_name, is_exact=False): self.scene.remove_world_object(name) return True - def add_plane(self, group_name, plane_name, plane_pose, plane_normal, auto_suffix=False): + def add_plane( + self, group_name, plane_name, plane_pose, plane_normal, auto_suffix=False + ): group = self._get_group_by_name(group_name) - if plane_name == '': - plane_name = 'plane' + if plane_name == "": + plane_name = "plane" if auto_suffix: plane_name += str(self._obj_suffix) self._obj_suffix += 1 - plane_pose_stamped = GeometryMsg.PoseStamped() + plane_pose_stamped = geo_msg.PoseStamped() plane_pose_stamped.pose = plane_pose plane_pose_stamped.header.frame_id = group.get_planning_frame() - self.scene.add_plane(plane_name, plane_pose_stamped, normal=(plane_normal.x, plane_normal.y, plane_normal.z)) + self.scene.add_plane( + plane_name, + plane_pose_stamped, + normal=(plane_normal.x, plane_normal.y, plane_normal.z), + ) return True @staticmethod - def get_group_orientation_constraints(group, constraint=''): + def get_group_orientation_constraints(group, constraint=""): """Generate an orientation constraint object for a given planning group based on the constraint descriptors. @@ -691,26 +756,26 @@ def get_group_orientation_constraints(group, constraint=''): :return: moveit_msgs.Constraints """ ref_pose = group.get_current_pose().pose - constraints = MoveItMsg.Constraints() + constraints = moveit_msg.Constraints() - oc = MoveItMsg.OrientationConstraint() + oc = moveit_msg.OrientationConstraint() oc.orientation.x = ref_pose.orientation.x oc.orientation.y = ref_pose.orientation.y oc.orientation.z = ref_pose.orientation.z oc.orientation.w = ref_pose.orientation.w no_constraint = math.pi * 4 - if 'r' in constraint: + if "r" in constraint: oc.absolute_x_axis_tolerance = 0.2 # roll else: oc.absolute_x_axis_tolerance = no_constraint - if 'p' in constraint: + if "p" in constraint: oc.absolute_y_axis_tolerance = 0.2 # pitch else: oc.absolute_y_axis_tolerance = no_constraint - if 'y' in constraint: + if "y" in constraint: oc.absolute_z_axis_tolerance = 0.2 # yaw else: oc.absolute_z_axis_tolerance = no_constraint diff --git a/src/rotools/moveit/core/server.py b/src/rotools/moveit/core/server.py index 12bd694..2753c9a 100644 --- a/src/rotools/moveit/core/server.py +++ b/src/rotools/moveit/core/server.py @@ -20,46 +20,78 @@ def __init__(self, kwargs): self.interface = interface.MoveGroupInterface(**kwargs) # MoveIt information getters - self._srv_get_names = rospy.Service('get_all_group_names', GetAllNames, self.get_all_names_handle) - self._srv_get_act_names = rospy.Service('get_active_group_names', GetAllNames, self.get_active_names_handle) - self._srv_get_pose = rospy.Service('get_group_pose', GetGroupPose, self.get_pose_handle) - self._srv_get_js = rospy.Service('get_group_joint_states', GetGroupJointStates, self.get_js_handle) + self._srv_get_names = rospy.Service( + "get_all_group_names", GetAllNames, self.get_all_names_handle + ) + self._srv_get_act_names = rospy.Service( + "get_active_group_names", GetAllNames, self.get_active_names_handle + ) + self._srv_get_pose = rospy.Service( + "get_group_pose", GetGroupPose, self.get_pose_handle + ) + self._srv_get_js = rospy.Service( + "get_group_joint_states", GetGroupJointStates, self.get_js_handle + ) # Robot movement executors # For a group self._srv_group_position = rospy.Service( - 'execute_group_position', ExecuteGroupPosition, self.group_position_handle + "execute_group_position", ExecuteGroupPosition, self.group_position_handle + ) + self._srv_group_shift = rospy.Service( + "execute_group_shift", ExecuteGroupShift, self.group_shift_handle + ) + self._srv_group_pose = rospy.Service( + "execute_group_pose", ExecuteGroupPose, self.group_pose_handle + ) + self._srv_group_js = rospy.Service( + "execute_group_joint_states", ExecuteGroupJointStates, self.group_js_handle ) - self._srv_group_shift = rospy.Service('execute_group_shift', ExecuteGroupShift, self.group_shift_handle) - self._srv_group_pose = rospy.Service('execute_group_pose', ExecuteGroupPose, self.group_pose_handle) - self._srv_group_js = rospy.Service('execute_group_joint_states', ExecuteGroupJointStates, self.group_js_handle) self._srv_group_home = rospy.Service( - 'execute_group_named_states', ExecuteGroupNamedStates, self.group_named_states_handle + "execute_group_named_states", + ExecuteGroupNamedStates, + self.group_named_states_handle, + ) + self._srv_group_plan = rospy.Service( + "execute_group_plan", ExecuteGroupPlan, self.group_plan_handle ) - self._srv_group_plan = rospy.Service('execute_group_plan', ExecuteGroupPlan, self.group_plan_handle) # For multiple groups - self._srv_all_pose = rospy.Service('execute_all_poses_py', ExecuteAllPoses, self.all_poses_handle) + self._srv_all_pose = rospy.Service( + "execute_all_poses_py", ExecuteAllPoses, self.all_poses_handle + ) # Planning scene helpers - self._srv_add_box = rospy.Service('execute_add_collision_box', ExecuteAddCollisionBox, self.add_box_handle) + self._srv_add_box = rospy.Service( + "execute_add_collision_box", ExecuteAddCollisionBox, self.add_box_handle + ) self._srv_add_plane = rospy.Service( - 'execute_add_collision_plane', ExecuteAddCollisionPlane, self.add_plane_handle + "execute_add_collision_plane", + ExecuteAddCollisionPlane, + self.add_plane_handle, ) self._srv_attach_box = rospy.Service( - 'execute_attach_collision_box', ExecuteAttachCollisionBox, self.attach_box_handle + "execute_attach_collision_box", + ExecuteAttachCollisionBox, + self.attach_box_handle, ) self._srv_detach_obj = rospy.Service( - 'execute_detach_collision', ExecuteDetachCollision, self.detach_object_handle + "execute_detach_collision", + ExecuteDetachCollision, + self.detach_object_handle, ) self._srv_remove_obj = rospy.Service( - 'execute_remove_collision', ExecuteRemoveCollision, self.remove_object_handle + "execute_remove_collision", + ExecuteRemoveCollision, + self.remove_object_handle, ) # General utilities - self._srv_get_prepare_pose = rospy.Service('get_prepare_pose', GetPreparePose, self.prepare_pose_handle) + self._srv_get_prepare_pose = rospy.Service( + "get_prepare_pose", GetPreparePose, self.prepare_pose_handle + ) self._srv_get_transformed_pose = rospy.Service( - 'get_transformed_pose', GetTransformedPose, self.transformed_pose_handle + "get_transformed_pose", GetTransformedPose, self.transformed_pose_handle ) def get_all_names_handle(self, req): @@ -86,7 +118,9 @@ def get_pose_handle(self, req): resp = GetGroupPoseResponse() try: resp.pose = self.interface.get_current_pose_of_group(req.group_name) - resp.ee_link, resp.ref_link = self.interface.get_frame_of_group(req.group_name) + resp.ee_link, resp.ref_link = self.interface.get_frame_of_group( + req.group_name + ) resp.result_status = resp.SUCCEEDED except IndexError: resp.result_status = resp.FAILED @@ -96,7 +130,9 @@ def get_js_handle(self, req): resp = GetGroupJointStatesResponse() try: joint_names = self.interface.get_active_joint_names_of_group(req.group_name) - joint_states = self.interface.get_joint_states_of_group(req.group_name, req.result_type) + joint_states = self.interface.get_joint_states_of_group( + req.group_name, req.result_type + ) resp.joint_names = joint_names resp.joint_states = joint_states resp.result_status = resp.SUCCEEDED @@ -114,11 +150,15 @@ def group_position_handle(self, req): if not req.tolerance: req.tolerance = 0.01 if req.goal_type == req.BASE_ABS: - ok = self.interface.group_go_to_global_base_position(req.group_name, req.goal, req.tolerance) + ok = self.interface.group_go_to_global_base_position( + req.group_name, req.goal, req.tolerance + ) elif req.goal_type == req.BASE_REL: ok = False else: - ok = self.interface.group_go_to_eef_position(req.group_name, req.goal, req.tolerance) + ok = self.interface.group_go_to_eef_position( + req.group_name, req.goal, req.tolerance + ) resp = ExecuteGroupPositionResponse() resp.result_status = resp.SUCCEEDED if ok else resp.FAILED return resp @@ -127,13 +167,19 @@ def group_pose_handle(self, req): if not req.tolerance: req.tolerance = 0.01 if req.goal_type == req.BASE_ABS: - ok = self.interface.group_goto_pose_goal_abs(req.group_name, req.goal, req.tolerance, req.constraint) + ok = self.interface.group_goto_pose_goal_abs( + req.group_name, req.goal, req.tolerance, req.constraint + ) elif req.goal_type == req.BASE_REL: - ok = self.interface.group_goto_pose_goal_rel(req.group_name, req.goal, req.tolerance, req.constraint) + ok = self.interface.group_goto_pose_goal_rel( + req.group_name, req.goal, req.tolerance, req.constraint + ) elif req.goal_type == req.EEF: - ok = self.interface.group_goto_pose_goal_eef(req.group_name, req.goal, req.tolerance, req.constraint) + ok = self.interface.group_goto_pose_goal_eef( + req.group_name, req.goal, req.tolerance, req.constraint + ) else: - rospy.logerr('Unknown goal type for group pose: {}'.format(req.goal_type)) + rospy.logerr("Unknown goal type for group pose: {}".format(req.goal_type)) ok = False resp = ExecuteGroupPoseResponse() resp.result_status = resp.SUCCEEDED if ok else resp.FAILED @@ -142,7 +188,9 @@ def group_pose_handle(self, req): def group_js_handle(self, req): if not req.tolerance: req.tolerance = 0.01 - ok = self.interface.group_go_to_joint_states(req.group_name, req.goal, req.tolerance) + ok = self.interface.group_go_to_joint_states( + req.group_name, req.goal, req.tolerance + ) resp = ExecuteGroupJointStatesResponse() resp.result_status = resp.SUCCEEDED if ok else resp.FAILED return resp @@ -155,11 +203,13 @@ def group_named_states_handle(self, req): def group_plan_handle(self, req): if req.is_absolute: - plan = self.interface.build_absolute_path_for_group(req.group_name, req.poses, req.stamp, - not req.allow_collision) + plan = self.interface.build_absolute_path_for_group( + req.group_name, req.poses, req.stamp, not req.allow_collision + ) else: - plan = self.interface.build_relative_path_for_group(req.group_name, req.poses, req.stamp, - not req.allow_collision) + plan = self.interface.build_relative_path_for_group( + req.group_name, req.poses, req.stamp, not req.allow_collision + ) ok = self.interface.execute_plan_for_group(req.group_name, plan) resp = ExecuteGroupPlanResponse() @@ -168,14 +218,29 @@ def group_plan_handle(self, req): def all_poses_handle(self, req): if req.goal_type == req.BASE_ABS: - ok = self.interface.all_goto_pose_goals_abs(req.group_names, req.goals, req.tolerance, - req.stamps, req.allow_collision) + ok = self.interface.all_goto_pose_goals_abs( + req.group_names, + req.goals, + req.tolerance, + req.stamps, + req.allow_collision, + ) elif req.goal_type == req.BASE_REL: - ok = self.interface.all_goto_pose_goals_rel(req.group_names, req.goals, req.tolerance, - req.stamps, req.allow_collision) + ok = self.interface.all_goto_pose_goals_rel( + req.group_names, + req.goals, + req.tolerance, + req.stamps, + req.allow_collision, + ) elif req.goal_type == req.EEF: - ok = self.interface.all_goto_pose_goals_eef(req.group_names, req.goals, req.tolerance, - req.stamps, req.allow_collision) + ok = self.interface.all_goto_pose_goals_eef( + req.group_names, + req.goals, + req.tolerance, + req.stamps, + req.allow_collision, + ) else: raise NotImplementedError resp = ExecuteAllPosesResponse() @@ -193,20 +258,30 @@ def all_poses_handle(self, req): # return resp def add_box_handle(self, req): - ok = self.interface.add_box(req.group_name, req.box_name, req.box_pose, - req.box_size, req.is_absolute, req.auto_suffix) + ok = self.interface.add_box( + req.group_name, + req.box_name, + req.box_pose, + req.box_size, + req.is_absolute, + req.auto_suffix, + ) resp = ExecuteAddCollisionBoxResponse() resp.result_status = resp.SUCCEEDED if ok else resp.FAILED return resp def attach_box_handle(self, req): - ok = self.interface.attach_box(req.group_name, req.eef_group_name, req.box_name, req.box_pose, req.box_size) + ok = self.interface.attach_box( + req.group_name, req.eef_group_name, req.box_name, req.box_pose, req.box_size + ) resp = ExecuteAttachCollisionBoxResponse() resp.result_status = resp.SUCCEEDED if ok else resp.FAILED return resp def add_plane_handle(self, req): - ok = self.interface.add_plane(req.group_name, req.plane_name, req.plane_pose, req.plane_normal) + ok = self.interface.add_plane( + req.group_name, req.plane_name, req.plane_pose, req.plane_normal + ) resp = ExecuteAddCollisionPlaneResponse() resp.result_status = resp.SUCCEEDED if ok else resp.FAILED return resp @@ -231,7 +306,9 @@ def prepare_pose_handle(self, req): return resp def transformed_pose_handle(self, req): - ok, pose = self.interface.get_transformed_pose(req.pose, req.source_frame, req.target_frame) + ok, pose = self.interface.get_transformed_pose( + req.pose, req.source_frame, req.target_frame + ) resp = GetTransformedPoseResponse() resp.result_status = resp.SUCCEEDED if ok else resp.FAILED resp.trans_pose = pose diff --git a/src/rotools/optitrack/linux/client.py b/src/rotools/optitrack/linux/client.py index 45d570c..5445c7b 100644 --- a/src/rotools/optitrack/linux/client.py +++ b/src/rotools/optitrack/linux/client.py @@ -13,8 +13,8 @@ def data_process(data): - position = re.findall(r'Position\s*:\s*(.*)', data)[0] - orientation = re.findall(r'Orientation\s*:\s*(.*)', data)[0] + position = re.findall(r"Position\s*:\s*(.*)", data)[0] + orientation = re.findall(r"Orientation\s*:\s*(.*)", data)[0] return eval(position), eval(orientation) @@ -23,24 +23,26 @@ class OptiTrackClient(object): def __init__(self, kwargs): self._client = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self._client.settimeout(20.) # in seconds - self._client.connect((kwargs['ip'], kwargs['port'])) - rospy.loginfo("Connected to socket: {}:{}".format(kwargs['ip'], kwargs['port'])) + self._client.settimeout(20.0) # in seconds + self._client.connect((kwargs["ip"], kwargs["port"])) + rospy.loginfo("Connected to socket: {}:{}".format(kwargs["ip"], kwargs["port"])) - self.timer = rospy.Timer(rospy.Duration.from_sec(1. / kwargs['rate']), self._socket_cb) + self.timer = rospy.Timer( + rospy.Duration.from_sec(1.0 / kwargs["rate"]), self._socket_cb + ) self._advertise_dict = {} - if kwargs['pose_topic'] is not None: - topics = kwargs['pose_topic'] + if kwargs["pose_topic"] is not None: + topics = kwargs["pose_topic"] self.register_topic(topics, geometry_msgs.msg.Pose) - if kwargs['odom_topic'] is not None: - topics = kwargs['odom_topic'] + if kwargs["odom_topic"] is not None: + topics = kwargs["odom_topic"] self.register_topic(topics, nav_msgs.msg.Odometry) - if kwargs['transform'] is not None: - self._transform = sd_pose(kwargs['transform'], check=True) + if kwargs["transform"] is not None: + self._transform = sd_pose(kwargs["transform"], check=True) else: self._transform = sd_pose([0, 0, 0, 0, 0, 0, 1], check=True) @@ -64,10 +66,12 @@ def create_publisher(topic_id, msg_type): return rospy.Publisher(topic_id, msg_type, queue_size=1) def _socket_cb(self, _): - utf_data = self._client.recv(1024).decode('utf-8') + utf_data = self._client.recv(1024).decode("utf-8") raw_position, raw_orientation = data_process(utf_data) raw_pose = sd_pose(raw_position + raw_orientation, check=True) - transformed_pose = to_ros_pose(get_transform_same_target(raw_pose, self._transform)) + transformed_pose = to_ros_pose( + get_transform_same_target(raw_pose, self._transform) + ) for _, entity in self._advertise_dict.items(): msg_type, publisher = entity if msg_type is geometry_msgs.msg.Pose: @@ -81,4 +85,4 @@ def _socket_cb(self, _): else: raise NotImplementedError publisher.publish(msg) - self._client.send('ok'.encode('utf-8')) + self._client.send("ok".encode("utf-8")) diff --git a/src/rotools/optitrack/windows/DataDescriptions.py b/src/rotools/optitrack/windows/DataDescriptions.py index fb87b98..c29f7da 100755 --- a/src/rotools/optitrack/windows/DataDescriptions.py +++ b/src/rotools/optitrack/windows/DataDescriptions.py @@ -74,7 +74,7 @@ def test_hash2(test_name, test_hash_str, test_object, run_test=True): out_str2 = "%sERROR: test_object was None" % indent_string else: obj_out_hash_str = "" - if str(type(test_object)) != 'NoneType': + if str(type(test_object)) != "NoneType": obj_out_str = test_object.get_as_string() obj_out_hash_str = hashlib.sha1(obj_out_str.encode()).hexdigest() @@ -82,7 +82,10 @@ def test_hash2(test_name, test_hash_str, test_object, run_test=True): out_str = "PASS" ret_value = K_PASS else: - out_str2 += "%s%s test_hash_str != out_hash_str\n" % (indent_string, test_name) + out_str2 += "%s%s test_hash_str != out_hash_str\n" % ( + indent_string, + test_name, + ) out_str2 += "%stest_hash_str=%s\n" % (indent_string, test_hash_str) out_str2 += "%sobj_out_hash_str=%s\n" % (indent_string, obj_out_hash_str) out_str2 += "%sobj_out_str =\n%s" % (indent_string, obj_out_str) @@ -98,7 +101,7 @@ def get_as_string(input_str): if type(input_str) == str: return input_str else: - return input_str.decode('utf-8') + return input_str.decode("utf-8") def get_data_sub_packet_type(new_data): @@ -144,11 +147,18 @@ def get_as_string(self, tab_str=" ", level=0): out_tab_str2 = get_tab_str(tab_str, level + 1) out_tab_str3 = get_tab_str(tab_str, level + 2) out_string = "" - out_string += "%sMarker Set Name: %s\n" % (out_tab_str, get_as_string(self.marker_set_name)) + out_string += "%sMarker Set Name: %s\n" % ( + out_tab_str, + get_as_string(self.marker_set_name), + ) num_markers = len(self.marker_names_list) out_string += "%sMarker Count : %d\n" % (out_tab_str2, num_markers) for i in range(num_markers): - out_string += "%s%3.1d Marker Name: %s\n" % (out_tab_str3, i, get_as_string(self.marker_names_list[i])) + out_string += "%s%3.1d Marker Name: %s\n" % ( + out_tab_str3, + i, + get_as_string(self.marker_names_list[i]), + ) return out_string @@ -161,8 +171,14 @@ def __init__(self, marker_name="", active_label=0, pos=[0.0, 0.0, 0.0]): def get_as_string(self, tab_str=" ", level=0): out_tab_str = get_tab_str(tab_str, level) out_string = "" - out_string += "%sMarker Label: %s Position: [%f %f %f] %s\n" % \ - (out_tab_str, self.active_label, self.pos[0], self.pos[1], self.pos[2], self.marker_name) + out_string += "%sMarker Label: %s Position: [%f %f %f] %s\n" % ( + out_tab_str, + self.active_label, + self.pos[0], + self.pos[1], + self.pos[2], + self.marker_name, + ) return out_string @@ -197,16 +213,27 @@ def get_as_string(self, tab_str=" ", level=0): out_tab_str = get_tab_str(tab_str, level) out_tab_str2 = get_tab_str(tab_str, level + 1) out_string = "" - out_string += "%sRigid Body Name : %s\n" % (out_tab_str, get_as_string(self.sz_name)) + out_string += "%sRigid Body Name : %s\n" % ( + out_tab_str, + get_as_string(self.sz_name), + ) out_string += "%sID : %d\n" % (out_tab_str, self.id_num) out_string += "%sParent ID : %d\n" % (out_tab_str, self.parent_id) out_string += "%sPosition : [%3.2f, %3.2f, %3.2f]\n" % ( - out_tab_str, self.pos[0], self.pos[1], self.pos[2]) + out_tab_str, + self.pos[0], + self.pos[1], + self.pos[2], + ) num_markers = len(self.rb_marker_list) out_string += "%sNumber of Markers : %d\n" % (out_tab_str, num_markers) # loop over markers for i in range(num_markers): - out_string += "%s%i %s" % (out_tab_str2, i, self.rb_marker_list[i].get_as_string(tab_str, 0)) + out_string += "%s%i %s" % ( + out_tab_str2, + i, + self.rb_marker_list[i].get_as_string(tab_str, 0), + ) return out_string @@ -230,13 +257,18 @@ def get_as_string(self, tab_str=" ", level=0): out_tab_str = get_tab_str(tab_str, level) out_tab_str2 = get_tab_str(tab_str, level + 1) out_string = "" - out_string += "%sName : %s\n" % (out_tab_str, get_as_string(self.name)) + out_string += "%sName : %s\n" % ( + out_tab_str, + get_as_string(self.name), + ) out_string += "%sID : %d\n" % (out_tab_str, self.id_num) num_bones = len(self.rigid_body_description_list) out_string += "%sRigid Body (Bone) Count : %d\n" % (out_tab_str, num_bones) for i in range(num_bones): out_string += "%sRigid Body (Bone) %d\n" % (out_tab_str2, i) - out_string += self.rigid_body_description_list[i].get_as_string(tab_str, level + 2) + out_string += self.rigid_body_description_list[i].get_as_string( + tab_str, level + 2 + ) return out_string @@ -289,14 +321,25 @@ def get_cal_matrix_as_string(self, tab_str="", level=0): out_string = "" out_string += "%sCal Matrix:\n" % out_tab_str for i in range(0, 12): - out_string += "%s%2.1d %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e\n" % \ - (out_tab_str2, i, - self.cal_matrix[i][0], self.cal_matrix[i][1], - self.cal_matrix[i][2], self.cal_matrix[i][3], - self.cal_matrix[i][4], self.cal_matrix[i][5], - self.cal_matrix[i][6], self.cal_matrix[i][7], - self.cal_matrix[i][8], self.cal_matrix[i][9], - self.cal_matrix[i][10], self.cal_matrix[i][11]) + out_string += ( + "%s%2.1d %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e\n" + % ( + out_tab_str2, + i, + self.cal_matrix[i][0], + self.cal_matrix[i][1], + self.cal_matrix[i][2], + self.cal_matrix[i][3], + self.cal_matrix[i][4], + self.cal_matrix[i][5], + self.cal_matrix[i][6], + self.cal_matrix[i][7], + self.cal_matrix[i][8], + self.cal_matrix[i][9], + self.cal_matrix[i][10], + self.cal_matrix[i][11], + ) + ) return out_string def get_corners_as_string(self, tab_str="", level=0): @@ -307,9 +350,13 @@ def get_corners_as_string(self, tab_str="", level=0): out_string = "" out_string += "%sCorners:\n" % out_tab_str for i in range(0, 4): - out_string += "%s%2.1d %3.3e %3.3e %3.3e\n" % \ - (out_tab_str2, i, - self.corners[i][0], self.corners[i][1], self.corners[i][2]) + out_string += "%s%2.1d %3.3e %3.3e %3.3e\n" % ( + out_tab_str2, + i, + self.corners[i][0], + self.corners[i][1], + self.corners[i][2], + ) return out_string def get_as_string(self, tab_str=" ", level=0): @@ -317,25 +364,39 @@ def get_as_string(self, tab_str=" ", level=0): out_tab_str = get_tab_str(tab_str, level) out_string = "" out_string += "%sID : %d\n" % (out_tab_str, self.id_num) - out_string += "%sSerial Number : %s\n" % (out_tab_str, get_as_string(self.serial_number)) + out_string += "%sSerial Number : %s\n" % ( + out_tab_str, + get_as_string(self.serial_number), + ) out_string += "%sWidth : %3.2f\n" % (out_tab_str, self.width) out_string += "%sLength : %3.2f\n" % (out_tab_str, self.length) - out_string += "%sOrigin : %3.2f, %3.2f, %3.2f\n" % (out_tab_str, - self.position[0], - self.position[1], - self.position[2]) + out_string += "%sOrigin : %3.2f, %3.2f, %3.2f\n" % ( + out_tab_str, + self.position[0], + self.position[1], + self.position[2], + ) out_string += self.get_cal_matrix_as_string(tab_str, level) out_string += self.get_corners_as_string(tab_str, level) - out_string += "%sPlate Type : %d\n" % (out_tab_str, self.plate_type) - out_string += "%sChannel Data Type : %d\n" % (out_tab_str, self.channel_data_type) + out_string += "%sPlate Type : %d\n" % ( + out_tab_str, + self.plate_type, + ) + out_string += "%sChannel Data Type : %d\n" % ( + out_tab_str, + self.channel_data_type, + ) num_channels = len(self.channel_list) out_string += "%sNumber of Channels : %d\n" % (out_tab_str, num_channels) # Channel Names list of NoC strings out_tab_str2 = get_tab_str(tab_str, level + 1) for channel_num in range(num_channels): - out_string += "%sChannel Name %d: %s\n" % (out_tab_str2, channel_num, - get_as_string(self.channel_list[channel_num])) + out_string += "%sChannel Name %d: %s\n" % ( + out_tab_str2, + channel_num, + get_as_string(self.channel_list[channel_num]), + ) return out_string @@ -370,14 +431,27 @@ def get_as_string(self, tab_str=" ", level=0): out_tab_str2 = get_tab_str(tab_str, level + 1) out_string = "" out_string += "%sID : %5.1d\n" % (out_tab_str, self.id_num) - out_string += "%sName : %s\n" % (out_tab_str, get_as_string(self.name)) - out_string += "%sSerial Number : %s\n" % (out_tab_str, get_as_string(self.serial_number)) + out_string += "%sName : %s\n" % ( + out_tab_str, + get_as_string(self.name), + ) + out_string += "%sSerial Number : %s\n" % ( + out_tab_str, + get_as_string(self.serial_number), + ) out_string += "%sDevice Type : %d\n" % (out_tab_str, self.device_type) - out_string += "%sChannel Data Type : %d\n" % (out_tab_str, self.channel_data_type) + out_string += "%sChannel Data Type : %d\n" % ( + out_tab_str, + self.channel_data_type, + ) num_channels = len(self.channel_list) out_string += "%sNumber of Channels : %d\n" % (out_tab_str, num_channels) for i in range(num_channels): - out_string += "%sChannel %2.1d Name : %s\n" % (out_tab_str2, i, get_as_string(self.channel_list[i])) + out_string += "%sChannel %2.1d Name : %s\n" % ( + out_tab_str2, + i, + get_as_string(self.channel_list[i]), + ) return out_string @@ -394,19 +468,27 @@ def get_as_string(self, tab_str="..", level=0): out_tab_str = get_tab_str(tab_str, level) out_string = "" out_string += "%sName : %s\n" % (out_tab_str, get_as_string(self.name)) - out_string += "%sPosition : [%3.2f, %3.2f, %3.2f]\n" % \ - (out_tab_str, self.position[0], self.position[1], self.position[2]) - out_string += "%sOrientation : [%3.2f, %3.2f, %3.2f, %3.2f]\n" % \ - (out_tab_str, - self.orientation[0], self.orientation[1], - self.orientation[2], self.orientation[3]) + out_string += "%sPosition : [%3.2f, %3.2f, %3.2f]\n" % ( + out_tab_str, + self.position[0], + self.position[1], + self.position[2], + ) + out_string += "%sOrientation : [%3.2f, %3.2f, %3.2f, %3.2f]\n" % ( + out_tab_str, + self.orientation[0], + self.orientation[1], + self.orientation[2], + self.orientation[3], + ) return out_string # cDataDescriptions # Full data descriptions -class DataDescriptions(): +class DataDescriptions: """Data Descriptions class""" + order_num = 0 def __init__(self): @@ -466,7 +548,7 @@ def add_force_plate(self, new_force_plate): self.force_plate_list.append(copy.deepcopy(new_force_plate)) def add_device(self, newdevice): - """ add_device - Add a device""" + """add_device - Add a device""" order_name = self.generate_order_name() # generate order entry @@ -475,7 +557,7 @@ def add_device(self, newdevice): self.device_list.append(copy.deepcopy(newdevice)) def add_camera(self, newcamera): - """ Add a new camera """ + """Add a new camera""" order_name = self.generate_order_name() # generate order entry @@ -506,28 +588,24 @@ def add_data(self, new_data): def get_object_from_list(self, list_name, pos_num): """Determine list name and position of the object""" ret_value = None - if (list_name == "marker_set_list") and \ - (pos_num < len(self.marker_set_list)): + if (list_name == "marker_set_list") and (pos_num < len(self.marker_set_list)): ret_value = self.marker_set_list[pos_num] - elif (list_name == "rigid_body_list") and \ - (pos_num < len(self.rigid_body_list)): + elif (list_name == "rigid_body_list") and (pos_num < len(self.rigid_body_list)): ret_value = self.rigid_body_list[pos_num] - elif (list_name == "skeleton_list") and \ - (pos_num < len(self.skeleton_list)): + elif (list_name == "skeleton_list") and (pos_num < len(self.skeleton_list)): ret_value = self.skeleton_list[pos_num] - elif (list_name == "force_plate_list") and \ - (pos_num < len(self.force_plate_list)): + elif (list_name == "force_plate_list") and ( + pos_num < len(self.force_plate_list) + ): ret_value = self.force_plate_list[pos_num] - elif (list_name == "device_list") and \ - (pos_num < len(self.device_list)): + elif (list_name == "device_list") and (pos_num < len(self.device_list)): ret_value = self.device_list[pos_num] - elif (list_name == "camera_list") and \ - (pos_num < len(self.camera_list)): + elif (list_name == "camera_list") and (pos_num < len(self.camera_list)): ret_value = self.camera_list[pos_num] else: @@ -558,7 +636,12 @@ def get_as_string(self, tab_str=" ", level=0): if tmp_object is not None: out_string += tmp_object.get_as_string(tab_str, level + 2) else: - out_string += "%s%s %s %s not found\n" % (out_tab_str3, tmp_key, tmp_name, tmp_num) + out_string += "%s%s %s %s not found\n" % ( + out_tab_str3, + tmp_key, + tmp_name, + tmp_num, + ) out_string += "\n" i += 1 @@ -567,6 +650,7 @@ def get_as_string(self, tab_str=" ", level=0): # cDataDescriptions END + def generate_marker_set_description(set_num=0): """generate_marker_set_description - Testing functions""" marker_set_description = MarkerSetDescription() @@ -610,7 +694,9 @@ def generate_rigid_body_description(rbd_num=0): def generate_skeleton_description(skeleton_num=0): """generate_skeleton_description -Generate Test SkeletonDescription Data""" - skel_desc = SkeletonDescription("SkeletonDescription_%3.3d" % skeleton_num, skeleton_num) + skel_desc = SkeletonDescription( + "SkeletonDescription_%3.3d" % skeleton_num, skeleton_num + ) # generate some rigid bodies to add skel_desc.add_rigid_body_description(generate_rigid_body_description(0)) skel_desc.add_rigid_body_description(generate_rigid_body_description(1)) @@ -630,10 +716,7 @@ def generate_force_plate_description(force_plate_num=0): width = random.random() * 10 length = random.random() * 10 origin = [(random.random() * 100), (random.random() * 100), (random.random() * 100)] - corners = [[0.0, 0.0, 0.0], - [0.0, 1.0, 0.0], - [1.0, 1.0, 0.0], - [1.0, 0.0, 0.0]] + corners = [[0.0, 0.0, 0.0], [0.0, 1.0, 0.0], [1.0, 1.0, 0.0], [1.0, 0.0, 0.0]] fp_desc = ForcePlateDescription(fp_id, serial_number) fp_desc.set_dimensions(width, length) @@ -652,7 +735,9 @@ def generate_device_description(dev_num=0): serial_number = "SerialNumber%3.3d" % dev_num device_type = dev_num % 4 channel_data_type = dev_num % 5 - dev_desc = DeviceDescription(new_id, name, serial_number, device_type, channel_data_type) + dev_desc = DeviceDescription( + new_id, name, serial_number, device_type, channel_data_type + ) for i in range(channel_data_type + 3): dev_desc.add_channel_name("channel_name_%2.2d" % i) return dev_desc @@ -700,27 +785,62 @@ def test_all(run_test=True): """Test all the Data Description classes""" totals = [0, 0, 0] if run_test is True: - test_cases = [["Test Marker Set Description 0", "754fe535286ca84bd054d9aca5e9906ab9384d92", - "generate_marker_set_description(0)", True], - ["Test RB Marker 0", "0f2612abf2ce70e479d7b9912f646f12910b3310", - "generate_rb_marker(0)", True], - ["Test Rigid Body Description 0", "7a4e93dcda442c1d9c5dcc5c01a247e4a6c01b66", - "generate_rigid_body_description(0)", True], - ["Test Skeleton Description 0", "b4d1a031dd7c323e3d316b5312329881a6a552ca", - "generate_skeleton_description(0)", True], - ["Test Force Plate Description 0", "b385dd1096bdd9f521eb48bb9cbfb3414ea075bd", - "generate_force_plate_description(0)", True], - ["Test Device Description 0", "39b4fdda402bc73c0b1cd5c7f61599476aa9a926", - "generate_device_description(0)", True], - ["Test Camera Description 0", "614602c5d290bda3b288138d5e25516dd1e1e85a", - "generate_camera_description(0)", True], - ["Test Data Description 0", "e5f448d10087ac818a65934710a85fc7ebfdf89e", - "generate_data_descriptions(0)", True], - ] + test_cases = [ + [ + "Test Marker Set Description 0", + "754fe535286ca84bd054d9aca5e9906ab9384d92", + "generate_marker_set_description(0)", + True, + ], + [ + "Test RB Marker 0", + "0f2612abf2ce70e479d7b9912f646f12910b3310", + "generate_rb_marker(0)", + True, + ], + [ + "Test Rigid Body Description 0", + "7a4e93dcda442c1d9c5dcc5c01a247e4a6c01b66", + "generate_rigid_body_description(0)", + True, + ], + [ + "Test Skeleton Description 0", + "b4d1a031dd7c323e3d316b5312329881a6a552ca", + "generate_skeleton_description(0)", + True, + ], + [ + "Test Force Plate Description 0", + "b385dd1096bdd9f521eb48bb9cbfb3414ea075bd", + "generate_force_plate_description(0)", + True, + ], + [ + "Test Device Description 0", + "39b4fdda402bc73c0b1cd5c7f61599476aa9a926", + "generate_device_description(0)", + True, + ], + [ + "Test Camera Description 0", + "614602c5d290bda3b288138d5e25516dd1e1e85a", + "generate_camera_description(0)", + True, + ], + [ + "Test Data Description 0", + "e5f448d10087ac818a65934710a85fc7ebfdf89e", + "generate_data_descriptions(0)", + True, + ], + ] num_tests = len(test_cases) for i in range(num_tests): data = eval(test_cases[i][2]) - totals_tmp = test_hash2(test_cases[i][0], test_cases[i][1], data, test_cases[i][3]) + totals_tmp = test_hash2( + test_cases[i][0], test_cases[i][1], data, test_cases[i][3] + ) totals = add_lists(totals, totals_tmp) print("--------------------") diff --git a/src/rotools/optitrack/windows/MoCapData.py b/src/rotools/optitrack/windows/MoCapData.py index a0a6df3..68c8707 100755 --- a/src/rotools/optitrack/windows/MoCapData.py +++ b/src/rotools/optitrack/windows/MoCapData.py @@ -77,7 +77,7 @@ def test_hash2(test_name, test_hash_str, test_object, run_test=True): out_str2 = "%sERROR: test_object was None" % indent_string else: - if str(type(test_object)) != 'NoneType': + if str(type(test_object)) != "NoneType": obj_out_str = test_object.get_as_string() obj_out_hash_str = hashlib.sha1(obj_out_str.encode()).hexdigest() @@ -85,7 +85,10 @@ def test_hash2(test_name, test_hash_str, test_object, run_test=True): out_str = "PASS" ret_value = K_PASS else: - out_str2 += "%s%s test_hash_str != out_hash_str\n" % (indent_string, test_name) + out_str2 += "%s%s test_hash_str != out_hash_str\n" % ( + indent_string, + test_name, + ) out_str2 += "%stest_hash_str=%s\n" % (indent_string, test_hash_str) out_str2 += "%sobj_out_hash_str=%s\n" % (indent_string, obj_out_hash_str) out_str2 += "%sobj_out_str =\n%s" % (indent_string, obj_out_str) @@ -104,7 +107,7 @@ def get_as_string(input_str): elif type_input_str == "": return "" elif type_input_str == "": - return input_str.decode('utf-8') + return input_str.decode("utf-8") else: print("type_input_str = %s NOT HANDLED" % type_input_str) return input_str @@ -141,12 +144,21 @@ def get_as_string(self, tab_str=" ", level=0): out_tab_str2 = get_tab_str(tab_str, level + 1) out_str = "" if self.model_name != "": - out_str += "%sModel Name : %s\n" % (out_tab_str, get_as_string(self.model_name)) + out_str += "%sModel Name : %s\n" % ( + out_tab_str, + get_as_string(self.model_name), + ) marker_count = len(self.marker_pos_list) out_str += "%sMarker Count :%3.1d\n" % (out_tab_str, marker_count) for i in range(marker_count): pos = self.marker_pos_list[i] - out_str += "%sMarker %3.1d pos : [%3.2f,%3.2f,%3.2f]\n" % (out_tab_str2, i, pos[0], pos[1], pos[2]) + out_str += "%sMarker %3.1d pos : [%3.2f,%3.2f,%3.2f]\n" % ( + out_tab_str2, + i, + pos[0], + pos[1], + pos[2], + ) return out_str @@ -182,7 +194,10 @@ def get_as_string(self, tab_str=" ", level=0): # Unlabeled markers count (4 bytes) unlabeled_markers_count = self.unlabeled_markers.get_num_points() - out_str += "%sUnlabeled Markers Count:%3.1d\n" % (out_tab_str, unlabeled_markers_count) + out_str += "%sUnlabeled Markers Count:%3.1d\n" % ( + out_tab_str, + unlabeled_markers_count, + ) out_str += self.unlabeled_markers.get_as_string(tab_str, level + 1) return out_str @@ -198,7 +213,12 @@ def get_as_string(self, tab_str=" ", level=0): out_tab_str = get_tab_str(tab_str, level) out_str = "" - out_str += "%sPosition: [%3.2f %3.2f %3.2f]\n" % (out_tab_str, self.pos[0], self.pos[1], self.pos[2]) + out_str += "%sPosition: [%3.2f %3.2f %3.2f]\n" % ( + out_tab_str, + self.pos[0], + self.pos[1], + self.pos[2], + ) out_str += "%sID : %3.1d\n" % (out_tab_str, self.id_num) out_str += "%sSize : %3.1d\n" % (out_tab_str, self.size) return out_str @@ -226,9 +246,19 @@ def get_as_string(self, tab_str=0, level=0): # header out_str += "%sID : %3.1d\n" % (out_tab_str, self.id_num) # Position and orientation - out_str += "%sPosition : [%3.2f, %3.2f, %3.2f]\n" % (out_tab_str, self.pos[0], self.pos[1], self.pos[2]) + out_str += "%sPosition : [%3.2f, %3.2f, %3.2f]\n" % ( + out_tab_str, + self.pos[0], + self.pos[1], + self.pos[2], + ) out_str += "%sOrientation : [%3.2f, %3.2f, %3.2f, %3.2f]\n" % ( - out_tab_str, self.rot[0], self.rot[1], self.rot[2], self.rot[3]) + out_tab_str, + self.rot[0], + self.rot[1], + self.rot[2], + self.rot[3], + ) marker_count = len(self.rb_marker_list) marker_count_range = range(0, marker_count) @@ -244,9 +274,9 @@ def get_as_string(self, tab_str=0, level=0): out_str += "%sMarker Error : %3.2f\n" % (out_tab_str, self.error) # Valid Tracking - tf_string = 'False' + tf_string = "False" if self.tracking_valid: - tf_string = 'True' + tf_string = "True" out_str += "%sTracking Valid: %s\n" % (out_tab_str, tf_string) return out_str @@ -314,7 +344,9 @@ def get_as_string(self, tab_str=" ", level=0): out_str += "%sSkeleton Count: %3.1d\n" % (out_tab_str, skeleton_count) for skeleton_num in range(skeleton_count): out_str += "%sSkeleton %3.1d\n" % (out_tab_str2, skeleton_num) - out_str += self.skeleton_list[skeleton_num].get_as_string(tab_str, level + 2) + out_str += self.skeleton_list[skeleton_num].get_as_string( + tab_str, level + 2 + ) return out_str @@ -330,7 +362,7 @@ def __init__(self, new_id, pos, size=0.0, param=0, residual=0.0): def __decode_marker_id(self): model_id = self.id_num >> 16 - marker_id = self.id_num & 0x0000ffff + marker_id = self.id_num & 0x0000FFFF return model_id, marker_id def __decode_param(self): @@ -343,14 +375,25 @@ def get_as_string(self, tab_str, level): out_tab_str = get_tab_str(tab_str, level) model_id, marker_id = self.__decode_marker_id() out_str = "" - out_str += "%sID : [MarkerID: %3.1d] [ModelID: %3.1d]\n" % (out_tab_str, marker_id, model_id) + out_str += "%sID : [MarkerID: %3.1d] [ModelID: %3.1d]\n" % ( + out_tab_str, + marker_id, + model_id, + ) out_str += "%spos : [%3.2f, %3.2f, %3.2f]\n" % ( - out_tab_str, self.pos[0], self.pos[1], self.pos[2]) + out_tab_str, + self.pos[0], + self.pos[1], + self.pos[2], + ) out_str += "%ssize : [%3.2f]\n" % (out_tab_str, self.size) occluded, point_cloud_solved, model_solved = self.__decode_param() out_str += "%soccluded : [%3.1d]\n" % (out_tab_str, occluded) - out_str += "%spoint_cloud_solved : [%3.1d]\n" % (out_tab_str, point_cloud_solved) + out_str += "%spoint_cloud_solved : [%3.1d]\n" % ( + out_tab_str, + point_cloud_solved, + ) out_str += "%smodel_solved : [%3.1d]\n" % (out_tab_str, model_solved) out_str += "%serr : [%3.2f]\n" % (out_tab_str, self.residual) @@ -374,7 +417,10 @@ def get_as_string(self, tab_str=" ", level=0): out_str = "" labeled_marker_count = len(self.labeled_marker_list) - out_str += "%sLabeled Marker Count:%3.1d\n" % (out_tab_str, labeled_marker_count) + out_str += "%sLabeled Marker Count:%3.1d\n" % ( + out_tab_str, + labeled_marker_count, + ) for i in range(0, labeled_marker_count): out_str += "%sLabeled Marker %3.1d\n" % (out_tab_str2, i) labeled_marker = self.labeled_marker_list[i] @@ -500,7 +546,11 @@ def get_as_string(self, tab_str, level, device_num): num_channels = len(self.channel_data_list) out_str += "%sDevice %3.1d ID: %3.1d Num Channels: %3.1d\n" % ( - out_tab_str, device_num, self.id_num, num_channels) + out_tab_str, + device_num, + self.id_num, + num_channels, + ) for i in range(num_channels): out_str += self.channel_data_list[i].get_as_string(tab_str, level + 1, i) @@ -549,11 +599,20 @@ def get_as_string(self, tab_str=" ", level=0): if not self.timestamp == -1: out_str += "%sTimestamp : %3.2f\n" % (out_tab_str, self.timestamp) if not self.stamp_camera_mid_exposure == -1: - out_str += "%sMid-exposure timestamp : %3.1d\n" % (out_tab_str, self.stamp_camera_mid_exposure) + out_str += "%sMid-exposure timestamp : %3.1d\n" % ( + out_tab_str, + self.stamp_camera_mid_exposure, + ) if not self.stamp_data_received == -1: - out_str += "%sCamera data received timestamp : %3.1d\n" % (out_tab_str, self.stamp_data_received) + out_str += "%sCamera data received timestamp : %3.1d\n" % ( + out_tab_str, + self.stamp_data_received, + ) if not self.stamp_transmit == -1: - out_str += "%sTransmit timestamp : %3.1d\n" % (out_tab_str, self.stamp_transmit) + out_str += "%sTransmit timestamp : %3.1d\n" % ( + out_tab_str, + self.stamp_transmit, + ) return out_str @@ -598,7 +657,10 @@ def get_as_string(self, tab_str=" ", level=0): out_tab_str = get_tab_str(tab_str, level) out_str = "" - out_str += "%sMoCap Frame Begin\n%s-----------------\n" % (out_tab_str, out_tab_str) + out_str += "%sMoCap Frame Begin\n%s-----------------\n" % ( + out_tab_str, + out_tab_str, + ) if not self.prefix_data == None: out_str += self.prefix_data.get_as_string() else: @@ -639,13 +701,17 @@ def get_as_string(self, tab_str=" ", level=0): else: out_str += "%sNo Suffix Data Set\n" % (out_tab_str) - out_str += "%sMoCap Frame End\n%s-----------------\n" % (out_tab_str, out_tab_str) + out_str += "%sMoCap Frame End\n%s-----------------\n" % ( + out_tab_str, + out_tab_str, + ) return out_str # test program + def generate_prefix_data(frame_num=0): frame_prefix_data = FramePrefixData(frame_num) return frame_prefix_data @@ -658,13 +724,17 @@ def generate_label(label_base="label", label_num=0): def generate_position_srand(pos_num=0, frame_num=0): random.seed(pos_num + (frame_num * 1000)) - position = [(random.random() * 100), (random.random() * 100), (random.random() * 100)] + position = [ + (random.random() * 100), + (random.random() * 100), + (random.random() * 100), + ] return position def generate_marker_data(label_base, label_num, num_points=1): label = generate_label(label_base, label_num) - if ((label_base == None) or (label_base == "")): + if (label_base == None) or (label_base == ""): label = "" marker_data = MarkerData() marker_data.set_model_name(label) @@ -793,7 +863,9 @@ def generate_force_plate_data(frame_num=0): return force_plate_data -def generate_device_channel_data(frame_num=0, device_num=0, channel_num=0, num_frames=1): +def generate_device_channel_data( + frame_num=0, device_num=0, channel_num=0, num_frames=1 +): rseed = (frame_num * 100000) + (device_num * 10000) + (channel_num * 1000) random.seed(rseed) device_channel_data = DeviceChannelData() @@ -846,29 +918,68 @@ def generate_mocap_data(frame_num=0): def test_all(run_test=True): totals = [0, 0, 0] if run_test is True: - test_cases = [["Test Prefix Data 0", "bffba016d02cf2167780df31aee697e1ec746b4c", - "generate_prefix_data(0)", True], - ["Test Marker Set Data 0", "d2550194fed1b1fc525f4f4d06bf584f291f41c7", - "generate_marker_set_data(0)", True], - ["Test Rigid Body Data 0", "abd1a48a476eaa9b5c4fae6e705e03aa75f85624", - "generate_rigid_body_data(0)", True], - ["Test Skeleton Data 0", "1e36e3334e291cebfaa530d7aab2122d6983ecab", - "generate_skeleton_data(0)", True], - ["Test Labeled Marker Data 0", "25f3ee026c3c8fc716fbb05c34138ef5afd95d75", - "generate_labeled_marker_data(0)", True], - ["Test Force Plate Data 0", "b83d04a1b89169bdcefee3bc3951c3bdcb6b792e", - "generate_force_plate_data(0)", True], - ["Test Device Data 0", "be10f0b93a7ba3858dce976b7868c1f79fd719c3", - "generate_device_data(0)", True], - ["Test Suffix Data 0", "6aa02c434bdb53a418ae1b1f73317dc80a5f887d", - "generate_suffix_data(0)", True], - ["Test MoCap Data 0", "09930ecf665d9eb3ca61616f9bcc55890373f414", - "generate_mocap_data(0)", True] - ] + test_cases = [ + [ + "Test Prefix Data 0", + "bffba016d02cf2167780df31aee697e1ec746b4c", + "generate_prefix_data(0)", + True, + ], + [ + "Test Marker Set Data 0", + "d2550194fed1b1fc525f4f4d06bf584f291f41c7", + "generate_marker_set_data(0)", + True, + ], + [ + "Test Rigid Body Data 0", + "abd1a48a476eaa9b5c4fae6e705e03aa75f85624", + "generate_rigid_body_data(0)", + True, + ], + [ + "Test Skeleton Data 0", + "1e36e3334e291cebfaa530d7aab2122d6983ecab", + "generate_skeleton_data(0)", + True, + ], + [ + "Test Labeled Marker Data 0", + "25f3ee026c3c8fc716fbb05c34138ef5afd95d75", + "generate_labeled_marker_data(0)", + True, + ], + [ + "Test Force Plate Data 0", + "b83d04a1b89169bdcefee3bc3951c3bdcb6b792e", + "generate_force_plate_data(0)", + True, + ], + [ + "Test Device Data 0", + "be10f0b93a7ba3858dce976b7868c1f79fd719c3", + "generate_device_data(0)", + True, + ], + [ + "Test Suffix Data 0", + "6aa02c434bdb53a418ae1b1f73317dc80a5f887d", + "generate_suffix_data(0)", + True, + ], + [ + "Test MoCap Data 0", + "09930ecf665d9eb3ca61616f9bcc55890373f414", + "generate_mocap_data(0)", + True, + ], + ] num_tests = len(test_cases) for i in range(num_tests): data = eval(test_cases[i][2]) - totals_tmp = test_hash2(test_cases[i][0], test_cases[i][1], data, test_cases[i][3]) + totals_tmp = test_hash2( + test_cases[i][0], test_cases[i][1], data, test_cases[i][3] + ) totals = add_lists(totals, totals_tmp) print("--------------------") diff --git a/src/rotools/optitrack/windows/NatNetClient.py b/src/rotools/optitrack/windows/NatNetClient.py index bacc534..99fcd6f 100755 --- a/src/rotools/optitrack/windows/NatNetClient.py +++ b/src/rotools/optitrack/windows/NatNetClient.py @@ -44,19 +44,19 @@ def trace_mf(*args): def get_message_id(data): - message_id = int.from_bytes(data[0:2], byteorder='little') + message_id = int.from_bytes(data[0:2], byteorder="little") return message_id # Create structs for reading various object types to speed up parsing. -Vector2 = struct.Struct('= 0: @@ -169,10 +171,12 @@ def set_nat_net_version(self, major, minor): # force frame send and play reset self.send_command("TimelinePlay") time.sleep(0.1) - tmpCommands = ["TimelinePlay", - "TimelineStop", - "SetPlaybackCurrentFrame,0", - "TimelineStop"] + tmpCommands = [ + "TimelinePlay", + "TimelineStop", + "SetPlaybackCurrentFrame,0", + "TimelineStop", + ] self.send_commands(tmpCommands, False) time.sleep(2) # reset to original output state @@ -186,7 +190,7 @@ def get_minor(self): return self.__nat_net_requested_version[1] def set_print_level(self, print_level=0): - if (print_level >= 0): + if print_level >= 0: self.print_level = print_level return self.print_level @@ -203,10 +207,12 @@ def connected(self): # check versions elif self.get_application_name() == "Not Set": ret_value = False - elif (self.__server_version[0] == 0) and \ - (self.__server_version[1] == 0) and \ - (self.__server_version[2] == 0) and \ - (self.__server_version[3] == 0): + elif ( + (self.__server_version[0] == 0) + and (self.__server_version[1] == 0) + and (self.__server_version[2] == 0) + and (self.__server_version[3] == 0) + ): ret_value = False return ret_value @@ -219,18 +225,20 @@ def __create_command_socket(self): # allow multiple clients on same machine to use multicast group address/port result.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) try: - result.bind(('', 0)) + result.bind(("", 0)) except socket.error as msg: print("ERROR: command socket error occurred:\n%s" % msg) - print("Check Motive/Server mode requested mode agreement. You requested Multicast ") + print( + "Check Motive/Server mode requested mode agreement. You requested Multicast " + ) result = None - except socket.herror: + except socket.herror: print("ERROR: command socket herror occurred") result = None - except socket.gaierror: + except socket.gaierror: print("ERROR: command socket gaierror occurred") result = None - except socket.timeout: + except socket.timeout: print("ERROR: command socket timeout occurred. Server not responding") result = None # set to broadcast mode @@ -239,12 +247,16 @@ def __create_command_socket(self): result.settimeout(2.0) else: # Unicast case - result = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + result = socket.socket( + socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP + ) try: result.bind((self.local_ip_address, 0)) except socket.error as msg: print("ERROR: command socket error occurred:\n%s" % msg) - print("Check Motive/Server mode requested mode agreement. You requested Unicast ") + print( + "Check Motive/Server mode requested mode agreement. You requested Unicast " + ) result = None except socket.herror: print("ERROR: command socket herror occurred") @@ -268,17 +280,23 @@ def __create_data_socket(self, port): if self.use_multicast: # Multicast case - result = socket.socket(socket.AF_INET, # Internet - socket.SOCK_DGRAM, - 0) # UDP + result = socket.socket( + socket.AF_INET, socket.SOCK_DGRAM, 0 # Internet + ) # UDP result.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - result.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, - socket.inet_aton(self.multicast_address) + socket.inet_aton(self.local_ip_address)) + result.setsockopt( + socket.IPPROTO_IP, + socket.IP_ADD_MEMBERSHIP, + socket.inet_aton(self.multicast_address) + + socket.inet_aton(self.local_ip_address), + ) try: result.bind((self.local_ip_address, port)) except socket.error as msg: print("ERROR: data socket error occurred:\n%s" % msg) - print(" Check Motive/Server mode requested mode agreement. You requested Multicast ") + print( + " Check Motive/Server mode requested mode agreement. You requested Multicast " + ) result = None except socket.herror: print("ERROR: data socket herror occurred") @@ -291,16 +309,18 @@ def __create_data_socket(self, port): result = None else: # Unicast case - result = socket.socket(socket.AF_INET, # Internet - socket.SOCK_DGRAM, - socket.IPPROTO_UDP) + result = socket.socket( + socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP # Internet + ) result.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # result.bind( (self.local_ip_address, port) ) try: - result.bind(('', 0)) + result.bind(("", 0)) except socket.error as msg: print("ERROR: data socket error occurred:\n%s" % msg) - print("Check Motive/Server mode requested mode agreement. You requested Unicast ") + print( + "Check Motive/Server mode requested mode agreement. You requested Unicast " + ) result = None except socket.herror: print("ERROR: data socket herror occurred") @@ -313,8 +333,12 @@ def __create_data_socket(self, port): result = None if self.multicast_address != "255.255.255.255": - result.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, - socket.inet_aton(self.multicast_address) + socket.inet_aton(self.local_ip_address)) + result.setsockopt( + socket.IPPROTO_IP, + socket.IP_ADD_MEMBERSHIP, + socket.inet_aton(self.multicast_address) + + socket.inet_aton(self.local_ip_address), + ) return result @@ -323,19 +347,22 @@ def __unpack_rigid_body(self, data, major, minor, rb_num): offset = 0 # ID (4 bytes) - new_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + new_id = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_mf("RB: %3.1d ID: %3.1d" % (rb_num, new_id)) # Position and orientation - pos = Vector3.unpack(data[offset:offset + 12]) + pos = Vector3.unpack(data[offset : offset + 12]) offset += 12 trace_mf("\tPosition : [%3.2f, %3.2f, %3.2f]" % (pos[0], pos[1], pos[2])) - rot = Quaternion.unpack(data[offset:offset + 16]) + rot = Quaternion.unpack(data[offset : offset + 16]) offset += 16 - trace_mf("\tOrientation : [%3.2f, %3.2f, %3.2f, %3.2f]" % (rot[0], rot[1], rot[2], rot[3])) + trace_mf( + "\tOrientation : [%3.2f, %3.2f, %3.2f, %3.2f]" + % (rot[0], rot[1], rot[2], rot[3]) + ) rigid_body = MoCapData.RigidBody(new_id, pos, rot) @@ -346,7 +373,7 @@ def __unpack_rigid_body(self, data, major, minor, rb_num): # RB Marker Data ( Before version 3.0. After Version 3.0 Marker data is in description ) if major < 3 and major != 0: # Marker count (4 bytes) - marker_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + marker_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 marker_count_range = range(0, marker_count) trace_mf("\tMarker Count:", marker_count) @@ -357,7 +384,7 @@ def __unpack_rigid_body(self, data, major, minor, rb_num): # Marker positions for i in marker_count_range: - pos = Vector3.unpack(data[offset:offset + 12]) + pos = Vector3.unpack(data[offset : offset + 12]) offset += 12 trace_mf("\tMarker", i, ":", pos[0], ",", pos[1], ",", pos[2]) rb_marker_list[i].pos = pos @@ -365,14 +392,16 @@ def __unpack_rigid_body(self, data, major, minor, rb_num): if major >= 2: # Marker ID's for i in marker_count_range: - new_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + new_id = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 trace_mf("\tMarker ID", i, ":", new_id) rb_marker_list[i].id = new_id # Marker sizes for i in marker_count_range: - size = FloatValue.unpack(data[offset:offset + 4]) + size = FloatValue.unpack(data[offset : offset + 4]) offset += 4 trace_mf("\tMarker Size", i, ":", size[0]) rb_marker_list[i].size = size @@ -380,19 +409,19 @@ def __unpack_rigid_body(self, data, major, minor, rb_num): for i in marker_count_range: rigid_body.add_rigid_body_marker(rb_marker_list[i]) if major >= 2: - marker_error, = FloatValue.unpack(data[offset:offset + 4]) + (marker_error,) = FloatValue.unpack(data[offset : offset + 4]) offset += 4 trace_mf("\tMarker Error: %3.2f" % marker_error) rigid_body.error = marker_error # Version 2.6 and later if ((major == 2) and (minor >= 6)) or major > 2: - param, = struct.unpack('h', data[offset:offset + 2]) + (param,) = struct.unpack("h", data[offset : offset + 2]) tracking_valid = (param & 0x01) != 0 offset += 2 - is_valid_str = 'False' + is_valid_str = "False" if tracking_valid: - is_valid_str = 'True' + is_valid_str = "True" trace_mf("\tTracking Valid: %s" % is_valid_str) if tracking_valid: rigid_body.tracking_valid = True @@ -405,16 +434,18 @@ def __unpack_rigid_body(self, data, major, minor, rb_num): def __unpack_skeleton(self, data, major, minor): offset = 0 - new_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + new_id = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_mf("ID:", new_id) skeleton = MoCapData.Skeleton(new_id) - rigid_body_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + rigid_body_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_mf("Rigid Body Count : %3.1d" % rigid_body_count) for rb_num in range(0, rigid_body_count): - offset_tmp, rigid_body = self.__unpack_rigid_body(data[offset:], major, minor, rb_num) + offset_tmp, rigid_body = self.__unpack_rigid_body( + data[offset:], major, minor, rb_num + ) skeleton.add_rigid_body(rigid_body) offset += offset_tmp @@ -424,7 +455,7 @@ def __unpack_skeleton(self, data, major, minor): def __unpack_frame_prefix_data(self, data): offset = 0 # Frame number (4 bytes) - frame_number = int.from_bytes(data[offset:offset + 4], byteorder='little') + frame_number = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_mf("Frame #:", frame_number) frame_prefix_data = MoCapData.FramePrefixData(frame_number) @@ -434,38 +465,44 @@ def __unpack_marker_set_data(self, data, packet_size, major, minor): marker_set_data = MoCapData.MarkerSetData() offset = 0 # Marker set count (4 bytes) - marker_set_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + marker_set_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_mf("Marker Set Count:", marker_set_count) for i in range(0, marker_set_count): marker_data = MoCapData.MarkerData() # Model name - model_name, separator, remainder = bytes(data[offset:]).partition(b'\0') + model_name, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(model_name) + 1 - trace_mf("Model Name : ", model_name.decode('utf-8')) + trace_mf("Model Name : ", model_name.decode("utf-8")) marker_data.set_model_name(model_name) # Marker count (4 bytes) - marker_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + marker_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_mf("Marker Count : ", marker_count) for j in range(0, marker_count): - pos = Vector3.unpack(data[offset:offset + 12]) + pos = Vector3.unpack(data[offset : offset + 12]) offset += 12 - trace_mf("\tMarker %3.1d : [%3.2f,%3.2f,%3.2f]" % (j, pos[0], pos[1], pos[2])) + trace_mf( + "\tMarker %3.1d : [%3.2f,%3.2f,%3.2f]" % (j, pos[0], pos[1], pos[2]) + ) marker_data.add_pos(pos) marker_set_data.add_marker_data(marker_data) # Unlabeled markers count (4 bytes) - unlabeled_markers_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + unlabeled_markers_count = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 trace_mf("Unlabeled Markers Count:", unlabeled_markers_count) for i in range(0, unlabeled_markers_count): - pos = Vector3.unpack(data[offset:offset + 12]) + pos = Vector3.unpack(data[offset : offset + 12]) offset += 12 - trace_mf("\tMarker %3.1d : [%3.2f,%3.2f,%3.2f]" % (i, pos[0], pos[1], pos[2])) + trace_mf( + "\tMarker %3.1d : [%3.2f,%3.2f,%3.2f]" % (i, pos[0], pos[1], pos[2]) + ) marker_set_data.add_unlabeled_marker(pos) return offset, marker_set_data @@ -473,12 +510,14 @@ def __unpack_rigid_body_data(self, data, packet_size, major, minor): rigid_body_data = MoCapData.RigidBodyData() offset = 0 # Rigid body count (4 bytes) - rigid_body_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + rigid_body_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_mf("Rigid Body Count:", rigid_body_count) for i in range(0, rigid_body_count): - offset_tmp, rigid_body = self.__unpack_rigid_body(data[offset:], major, minor, i) + offset_tmp, rigid_body = self.__unpack_rigid_body( + data[offset:], major, minor, i + ) offset += offset_tmp rigid_body_data.add_rigid_body(rigid_body) @@ -491,11 +530,15 @@ def __unpack_skeleton_data(self, data, packet_size, major, minor): # Version 2.1 and later skeleton_count = 0 if (major == 2 and minor > 0) or major > 2: - skeleton_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + skeleton_count = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 trace_mf("Skeleton Count:", skeleton_count) for _ in range(0, skeleton_count): - rel_offset, skeleton = self.__unpack_skeleton(data[offset:], major, minor) + rel_offset, skeleton = self.__unpack_skeleton( + data[offset:], major, minor + ) offset += rel_offset skeleton_data.add_skeleton(skeleton) @@ -505,7 +548,7 @@ def __decode_marker_id(self, new_id): model_id = 0 marker_id = 0 model_id = new_id >> 16 - marker_id = new_id & 0x0000ffff + marker_id = new_id & 0x0000FFFF return model_id, marker_id def __unpack_labeled_marker_data(self, data, packet_size, major, minor): @@ -514,27 +557,32 @@ def __unpack_labeled_marker_data(self, data, packet_size, major, minor): # Labeled markers (Version 2.3 and later) labeled_marker_count = 0 if (major == 2 and minor > 3) or major > 2: - labeled_marker_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + labeled_marker_count = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 trace_mf("Labeled Marker Count:", labeled_marker_count) for _ in range(0, labeled_marker_count): model_id = 0 marker_id = 0 - tmp_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + tmp_id = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 model_id, marker_id = self.__decode_marker_id(tmp_id) - pos = Vector3.unpack(data[offset:offset + 12]) + pos = Vector3.unpack(data[offset : offset + 12]) offset += 12 - size = FloatValue.unpack(data[offset:offset + 4]) + size = FloatValue.unpack(data[offset : offset + 4]) offset += 4 - trace_mf("ID : [MarkerID: %3.1d] [ModelID: %3.1d]" % (marker_id, model_id)) + trace_mf( + "ID : [MarkerID: %3.1d] [ModelID: %3.1d]" + % (marker_id, model_id) + ) trace_mf(" pos : [%3.2f, %3.2f, %3.2f]" % (pos[0], pos[1], pos[2])) trace_mf(" size : [%3.2f]" % size) # Version 2.6 and later param = 0 - if ((major == 2 and minor >= 6) or major > 2): - param, = struct.unpack('h', data[offset:offset + 2]) + if (major == 2 and minor >= 6) or major > 2: + (param,) = struct.unpack("h", data[offset : offset + 2]) offset += 2 # occluded = ( param & 0x01 ) != 0 # point_cloud_solved = ( param & 0x02 ) != 0 @@ -543,11 +591,13 @@ def __unpack_labeled_marker_data(self, data, packet_size, major, minor): # Version 3.0 and later residual = 0.0 if major >= 3: - residual, = FloatValue.unpack(data[offset:offset + 4]) + (residual,) = FloatValue.unpack(data[offset : offset + 4]) offset += 4 trace_mf(" err : [%3.2f]" % residual) - labeled_marker = MoCapData.LabeledMarker(tmp_id, pos, size, param, residual) + labeled_marker = MoCapData.LabeledMarker( + tmp_id, pos, size, param, residual + ) labeled_marker_data.add_labeled_marker(labeled_marker) return offset, labeled_marker_data @@ -559,34 +609,50 @@ def __unpack_force_plate_data(self, data, packet_size, major, minor): # Force Plate data (version 2.9 and later) force_plate_count = 0 if (major == 2 and minor >= 9) or major > 2: - force_plate_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + force_plate_count = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 trace_mf("Force Plate Count:", force_plate_count) for i in range(0, force_plate_count): # ID - force_plate_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + force_plate_id = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 force_plate = MoCapData.ForcePlate(force_plate_id) # Channel Count - force_plate_channel_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + force_plate_channel_count = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 - trace_mf("\tForce Plate %3.1d ID: %3.1d Num Channels: %3.1d" % ( - i, force_plate_id, force_plate_channel_count)) + trace_mf( + "\tForce Plate %3.1d ID: %3.1d Num Channels: %3.1d" + % (i, force_plate_id, force_plate_channel_count) + ) # Channel Data for j in range(force_plate_channel_count): fp_channel_data = MoCapData.ForcePlateChannelData() - force_plate_channel_frame_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + force_plate_channel_frame_count = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 out_string = "\tChannel %3.1d: " % (j) - out_string += " %3.1d Frames - Frame Data: " % (force_plate_channel_frame_count) + out_string += " %3.1d Frames - Frame Data: " % ( + force_plate_channel_frame_count + ) # Force plate frames - n_frames_show = min(force_plate_channel_frame_count, n_frames_show_max) + n_frames_show = min( + force_plate_channel_frame_count, n_frames_show_max + ) for k in range(force_plate_channel_frame_count): - force_plate_channel_val = FloatValue.unpack(data[offset:offset + 4]) + force_plate_channel_val = FloatValue.unpack( + data[offset : offset + 4] + ) offset += 4 fp_channel_data.add_frame_entry(force_plate_channel_val) @@ -594,7 +660,9 @@ def __unpack_force_plate_data(self, data, packet_size, major, minor): out_string += "%3.2f " % (force_plate_channel_val) if n_frames_show < force_plate_channel_frame_count: out_string += " showing %3.1d of %3.1d frames" % ( - n_frames_show, force_plate_channel_frame_count) + n_frames_show, + force_plate_channel_frame_count, + ) trace_mf("%s" % out_string) force_plate.add_channel_data(fp_channel_data) force_plate_data.add_force_plate(force_plate) @@ -607,41 +675,59 @@ def __unpack_device_data(self, data, packet_size, major, minor): # Device data (version 2.11 and later) device_count = 0 if (major == 2 and minor >= 11) or (major > 2): - device_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + device_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_mf("Device Count:", device_count) for i in range(0, device_count): # ID - device_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + device_id = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 device = MoCapData.Device(device_id) # Channel Count - device_channel_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + device_channel_count = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 - trace_mf("\tDevice %3.1d ID: %3.1d Num Channels: %3.1d" % (i, device_id, device_channel_count)) + trace_mf( + "\tDevice %3.1d ID: %3.1d Num Channels: %3.1d" + % (i, device_id, device_channel_count) + ) # Channel Data for j in range(0, device_channel_count): device_channel_data = MoCapData.DeviceChannelData() - device_channel_frame_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + device_channel_frame_count = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 out_string = "\tChannel %3.1d " % (j) - out_string += " %3.1d Frames - Frame Data: " % device_channel_frame_count + out_string += ( + " %3.1d Frames - Frame Data: " % device_channel_frame_count + ) # Device Frame Data n_frames_show = min(device_channel_frame_count, n_frames_show_max) for k in range(0, device_channel_frame_count): - device_channel_val = int.from_bytes(data[offset:offset + 4], byteorder='little') - device_channel_val = FloatValue.unpack(data[offset:offset + 4]) + device_channel_val = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) + device_channel_val = FloatValue.unpack( + data[offset : offset + 4] + ) offset += 4 if k < n_frames_show: out_string += "%3.2f " % device_channel_val device_channel_data.add_frame_entry(device_channel_val) if n_frames_show < device_channel_frame_count: - out_string += " showing %3.1d of %3.1d frames" % (n_frames_show, device_channel_frame_count) + out_string += " showing %3.1d of %3.1d frames" % ( + n_frames_show, + device_channel_frame_count, + ) trace_mf("%s" % out_string) device.add_channel_data(device_channel_data) device_data.add_device(device) @@ -652,43 +738,51 @@ def __unpack_frame_suffix_data(self, data, packet_size, major, minor): offset = 0 # Timecode - timecode = int.from_bytes(data[offset:offset + 4], byteorder='little') + timecode = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 frame_suffix_data.timecode = timecode - timecode_sub = int.from_bytes(data[offset:offset + 4], byteorder='little') + timecode_sub = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 frame_suffix_data.timecode_sub = timecode_sub # Timestamp (increased to double precision in 2.7 and later) if (major == 2 and minor >= 7) or (major > 2): - timestamp, = DoubleValue.unpack(data[offset:offset + 8]) + (timestamp,) = DoubleValue.unpack(data[offset : offset + 8]) offset += 8 else: - timestamp, = FloatValue.unpack(data[offset:offset + 4]) + (timestamp,) = FloatValue.unpack(data[offset : offset + 4]) offset += 4 trace_mf("Timestamp : %3.2f" % timestamp) frame_suffix_data.timestamp = timestamp # Hires Timestamp (Version 3.0 and later) if major >= 3: - stamp_camera_mid_exposure = int.from_bytes(data[offset:offset + 8], byteorder='little') - trace_mf("Mid-exposure timestamp : %3.1d" % stamp_camera_mid_exposure) + stamp_camera_mid_exposure = int.from_bytes( + data[offset : offset + 8], byteorder="little" + ) + trace_mf( + "Mid-exposure timestamp : %3.1d" % stamp_camera_mid_exposure + ) offset += 8 frame_suffix_data.stamp_camera_mid_exposure = stamp_camera_mid_exposure - stamp_data_received = int.from_bytes(data[offset:offset + 8], byteorder='little') + stamp_data_received = int.from_bytes( + data[offset : offset + 8], byteorder="little" + ) offset += 8 frame_suffix_data.stamp_data_received = stamp_data_received trace_mf("Camera data received timestamp : %3.1d" % stamp_data_received) - stamp_transmit = int.from_bytes(data[offset:offset + 8], byteorder='little') + stamp_transmit = int.from_bytes( + data[offset : offset + 8], byteorder="little" + ) offset += 8 trace_mf("Transmit timestamp : %3.1d" % stamp_transmit) frame_suffix_data.stamp_transmit = stamp_transmit # Frame parameters - param, = struct.unpack('h', data[offset:offset + 2]) + (param,) = struct.unpack("h", data[offset : offset + 2]) is_recording = (param & 0x01) != 0 tracked_models_changed = (param & 0x02) != 0 offset += 2 @@ -713,46 +807,57 @@ def __unpack_mocap_data(self, data: bytes, packet_size, major, minor): frame_number = frame_prefix_data.frame_number # Marker Set Data - rel_offset, marker_set_data = self.__unpack_marker_set_data(data[offset:], (packet_size - offset), major, minor) + rel_offset, marker_set_data = self.__unpack_marker_set_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_marker_set_data(marker_set_data) marker_set_count = marker_set_data.get_marker_set_count() unlabeled_markers_count = marker_set_data.get_unlabeled_marker_count() # Rigid Body Data - rel_offset, rigid_body_data = self.__unpack_rigid_body_data(data[offset:], (packet_size - offset), major, minor) + rel_offset, rigid_body_data = self.__unpack_rigid_body_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_rigid_body_data(rigid_body_data) rigid_body_count = rigid_body_data.get_rigid_body_count() # Skeleton Data - rel_offset, skeleton_data = self.__unpack_skeleton_data(data[offset:], (packet_size - offset), major, minor) + rel_offset, skeleton_data = self.__unpack_skeleton_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_skeleton_data(skeleton_data) skeleton_count = skeleton_data.get_skeleton_count() # Labeled Marker Data - rel_offset, labeled_marker_data = self.__unpack_labeled_marker_data(data[offset:], (packet_size - offset), - major, minor) + rel_offset, labeled_marker_data = self.__unpack_labeled_marker_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_labeled_marker_data(labeled_marker_data) labeled_marker_count = labeled_marker_data.get_labeled_marker_count() # Force Plate Data - rel_offset, force_plate_data = self.__unpack_force_plate_data(data[offset:], (packet_size - offset), major, - minor) + rel_offset, force_plate_data = self.__unpack_force_plate_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_force_plate_data(force_plate_data) # Device Data - rel_offset, device_data = self.__unpack_device_data(data[offset:], (packet_size - offset), major, minor) + rel_offset, device_data = self.__unpack_device_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_device_data(device_data) # Frame Suffix Data # rel_offset, timecode, timecode_sub, timestamp, is_recording, tracked_models_changed = \ - rel_offset, frame_suffix_data = self.__unpack_frame_suffix_data(data[offset:], (packet_size - offset), major, - minor) + rel_offset, frame_suffix_data = self.__unpack_frame_suffix_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_suffix_data(frame_suffix_data) @@ -786,18 +891,18 @@ def __unpack_marker_set_description(self, data, major, minor): offset = 0 - name, separator, remainder = bytes(data[offset:]).partition(b'\0') + name, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(name) + 1 - trace_dd("Marker Set Name: %s" % (name.decode('utf-8'))) + trace_dd("Marker Set Name: %s" % (name.decode("utf-8"))) ms_desc.set_name(name) - marker_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + marker_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_dd("Marker Count : %3.1d" % marker_count) for i in range(0, marker_count): - name, separator, remainder = bytes(data[offset:]).partition(b'\0') + name, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(name) + 1 - trace_dd("\t%2.1d Marker Name: %s" % (i, name.decode('utf-8'))) + trace_dd("\t%2.1d Marker Name: %s" % (i, name.decode("utf-8"))) ms_desc.add_marker_name(name) return offset, ms_desc @@ -809,34 +914,36 @@ def __unpack_rigid_body_description(self, data, major, minor): # Version 2.0 or higher if (major >= 2) or (major == 0): - name, separator, remainder = bytes(data[offset:]).partition(b'\0') + name, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(name) + 1 rb_desc.set_name(name) - trace_dd("\tRigid Body Name : ", name.decode('utf-8')) + trace_dd("\tRigid Body Name : ", name.decode("utf-8")) # ID - new_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + new_id = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 rb_desc.set_id(new_id) trace_dd("\tID : ", str(new_id)) # Parent ID - parent_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + parent_id = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 rb_desc.set_parent_id(parent_id) trace_dd("\tParent ID : ", parent_id) # Position Offsets - pos = Vector3.unpack(data[offset:offset + 12]) + pos = Vector3.unpack(data[offset : offset + 12]) offset += 12 rb_desc.set_pos(pos[0], pos[1], pos[2]) - trace_dd("\tPosition : [%3.2f, %3.2f, %3.2f]" % (pos[0], pos[1], pos[2])) + trace_dd( + "\tPosition : [%3.2f, %3.2f, %3.2f]" % (pos[0], pos[1], pos[2]) + ) # Version 3.0 and higher, rigid body marker information contained in description if (major >= 3) or (major == 0): # Marker Count - marker_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + marker_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_dd("\tNumber of Markers : ", marker_count) @@ -848,26 +955,39 @@ def __unpack_rigid_body_description(self, data, major, minor): marker_name = "" for marker in marker_count_range: # Offset - marker_offset = Vector3.unpack(data[offset1:offset1 + 12]) + marker_offset = Vector3.unpack(data[offset1 : offset1 + 12]) offset1 += 12 # Active Label - active_label = int.from_bytes(data[offset2:offset2 + 4], byteorder='little') + active_label = int.from_bytes( + data[offset2 : offset2 + 4], byteorder="little" + ) offset2 += 4 # Marker Name if (major >= 4) or (major == 0): # markername - marker_name, separator, remainder = bytes(data[offset3:]).partition(b'\0') - marker_name = marker_name.decode('utf-8') + marker_name, separator, remainder = bytes(data[offset3:]).partition( + b"\0" + ) + marker_name = marker_name.decode("utf-8") offset3 += len(marker_name) + 1 - rb_marker = DataDescriptions.RBMarker(marker_name, active_label, marker_offset) + rb_marker = DataDescriptions.RBMarker( + marker_name, active_label, marker_offset + ) rb_desc.add_rb_marker(rb_marker) - trace_dd("\t%3.1d Marker Label: %s Position: [%3.2f %3.2f %3.2f] %s" % (marker, active_label, \ - marker_offset[0], - marker_offset[1], - marker_offset[2], marker_name)) + trace_dd( + "\t%3.1d Marker Label: %s Position: [%3.2f %3.2f %3.2f] %s" + % ( + marker, + active_label, + marker_offset[0], + marker_offset[1], + marker_offset[2], + marker_name, + ) + ) offset = offset3 @@ -880,26 +1000,28 @@ def __unpack_skeleton_description(self, data, major, minor): offset = 0 # Name - name, separator, remainder = bytes(data[offset:]).partition(b'\0') + name, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(name) + 1 skeleton_desc.set_name(name) - trace_dd("Name : %s" % name.decode('utf-8')) + trace_dd("Name : %s" % name.decode("utf-8")) # ID - new_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + new_id = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 skeleton_desc.set_id(new_id) trace_dd("ID : %3.1d" % new_id) # # of RigidBodies - rigid_body_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + rigid_body_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_dd("Rigid Body (Bone) Count : %3.1d" % rigid_body_count) # Loop over all Rigid Bodies for i in range(0, rigid_body_count): trace_dd("Rigid Body (Bone) ", i) - offset_tmp, rb_desc_tmp = self.__unpack_rigid_body_description(data[offset:], major, minor) + offset_tmp, rb_desc_tmp = self.__unpack_rigid_body_description( + data[offset:], major, minor + ) offset += offset_tmp skeleton_desc.add_rigid_body_description(rb_desc_tmp) return offset, skeleton_desc @@ -910,77 +1032,72 @@ def __unpack_force_plate_description(self, data, major, minor): if major >= 3: fp_desc = DataDescriptions.ForcePlateDescription() # ID - new_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + new_id = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 fp_desc.set_id(new_id) trace_dd("\tID : ", str(new_id)) # Serial Number - serial_number, separator, remainder = bytes(data[offset:]).partition(b'\0') + serial_number, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(serial_number) + 1 fp_desc.set_serial_number(serial_number) - trace_dd("\tSerial Number : ", serial_number.decode('utf-8')) + trace_dd("\tSerial Number : ", serial_number.decode("utf-8")) # Dimensions - f_width = FloatValue.unpack(data[offset:offset + 4]) + f_width = FloatValue.unpack(data[offset : offset + 4]) offset += 4 trace_dd("\tWidth : %3.2f" % f_width) - f_length = FloatValue.unpack(data[offset:offset + 4]) + f_length = FloatValue.unpack(data[offset : offset + 4]) offset += 4 fp_desc.set_dimensions(f_width[0], f_length[0]) trace_dd("\tLength : %3.2f" % f_length) # Origin - origin = Vector3.unpack(data[offset:offset + 12]) + origin = Vector3.unpack(data[offset : offset + 12]) offset += 12 fp_desc.set_origin(origin[0], origin[1], origin[2]) - trace_dd("\tOrigin : %3.2f, %3.2f, %3.2f" % (origin[0], origin[1], origin[2])) + trace_dd( + "\tOrigin : %3.2f, %3.2f, %3.2f" % (origin[0], origin[1], origin[2]) + ) # Calibration Matrix 12x12 floats trace_dd("Cal Matrix:") cal_matrix_tmp = [[0.0 for col in range(12)] for row in range(12)] for i in range(0, 12): - cal_matrix_row = FPCalMatrixRow.unpack(data[offset:offset + (12 * 4)]) - trace_dd("\t%3.1d %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e" % (i - , - cal_matrix_row[ - 0], - cal_matrix_row[ - 1], - cal_matrix_row[ - 2], - cal_matrix_row[ - 3] - , - cal_matrix_row[ - 4], - cal_matrix_row[ - 5], - cal_matrix_row[ - 6], - cal_matrix_row[ - 7] - , - cal_matrix_row[ - 8], - cal_matrix_row[ - 9], - cal_matrix_row[ - 10], - cal_matrix_row[ - 11])) + cal_matrix_row = FPCalMatrixRow.unpack(data[offset : offset + (12 * 4)]) + trace_dd( + "\t%3.1d %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e %3.3e" + % ( + i, + cal_matrix_row[0], + cal_matrix_row[1], + cal_matrix_row[2], + cal_matrix_row[3], + cal_matrix_row[4], + cal_matrix_row[5], + cal_matrix_row[6], + cal_matrix_row[7], + cal_matrix_row[8], + cal_matrix_row[9], + cal_matrix_row[10], + cal_matrix_row[11], + ) + ) cal_matrix_tmp[i] = copy.deepcopy(cal_matrix_row) - offset += (12 * 4) + offset += 12 * 4 fp_desc.set_cal_matrix(cal_matrix_tmp) # Corners 4x3 floats - corners = FPCorners.unpack(data[offset:offset + (12 * 4)]) - offset += (12 * 4) + corners = FPCorners.unpack(data[offset : offset + (12 * 4)]) + offset += 12 * 4 o_2 = 0 trace_dd("Corners:") corners_tmp = [[0.0 for col in range(3)] for row in range(4)] for i in range(0, 4): - trace_dd("\t%3.1d %3.3e %3.3e %3.3e" % (i, corners[o_2], corners[o_2 + 1], corners[o_2 + 2])) + trace_dd( + "\t%3.1d %3.3e %3.3e %3.3e" + % (i, corners[o_2], corners[o_2 + 1], corners[o_2 + 2]) + ) corners_tmp[i][0] = corners[o_2] corners_tmp[i][1] = corners[o_2 + 1] corners_tmp[i][2] = corners[o_2 + 2] @@ -988,27 +1105,31 @@ def __unpack_force_plate_description(self, data, major, minor): fp_desc.set_corners(corners_tmp) # Plate Type int - plate_type = int.from_bytes(data[offset:offset + 4], byteorder='little') + plate_type = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 fp_desc.set_plate_type(plate_type) trace_dd("Plate Type : ", plate_type) # Channel Data Type int - channel_data_type = int.from_bytes(data[offset:offset + 4], byteorder='little') + channel_data_type = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 fp_desc.set_channel_data_type(channel_data_type) trace_dd("Channel Data Type : ", channel_data_type) # Number of Channels int - num_channels = int.from_bytes(data[offset:offset + 4], byteorder='little') + num_channels = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_dd("Number of Channels : ", num_channels) # Channel Names list of NoC strings for i in range(0, num_channels): - channel_name, separator, remainder = bytes(data[offset:]).partition(b'\0') + channel_name, separator, remainder = bytes(data[offset:]).partition( + b"\0" + ) offset += len(channel_name) + 1 - trace_dd("\tChannel Name %3.1d: %s" % (i, channel_name.decode('utf-8'))) + trace_dd("\tChannel Name %3.1d: %s" % (i, channel_name.decode("utf-8"))) fp_desc.add_channel_name(channel_name) trace_dd("unpackForcePlate processed ", offset, " bytes") @@ -1019,44 +1140,49 @@ def __unpack_device_description(self, data, major, minor): offset = 0 if major >= 3: # new_id - new_id = int.from_bytes(data[offset:offset + 4], byteorder='little') + new_id = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_dd("\tID : ", str(new_id)) # Name - name, separator, remainder = bytes(data[offset:]).partition(b'\0') + name, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(name) + 1 - trace_dd("\tName : ", name.decode('utf-8')) + trace_dd("\tName : ", name.decode("utf-8")) # Serial Number - serial_number, separator, remainder = bytes(data[offset:]).partition(b'\0') + serial_number, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(serial_number) + 1 - trace_dd("\tSerial Number : ", serial_number.decode('utf-8')) + trace_dd("\tSerial Number : ", serial_number.decode("utf-8")) # Device Type int - device_type = int.from_bytes(data[offset:offset + 4], byteorder='little') + device_type = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_dd("Device Type : ", device_type) # Channel Data Type int - channel_data_type = int.from_bytes(data[offset:offset + 4], byteorder='little') + channel_data_type = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 trace_dd("Channel Data Type : ", channel_data_type) - device_desc = DataDescriptions.DeviceDescription(new_id, name, serial_number, device_type, - channel_data_type) + device_desc = DataDescriptions.DeviceDescription( + new_id, name, serial_number, device_type, channel_data_type + ) # Number of Channels int - num_channels = int.from_bytes(data[offset:offset + 4], byteorder='little') + num_channels = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_dd("Number of Channels ", num_channels) # Channel Names list of NoC strings for i in range(0, num_channels): - channel_name, separator, remainder = bytes(data[offset:]).partition(b'\0') + channel_name, separator, remainder = bytes(data[offset:]).partition( + b"\0" + ) offset += len(channel_name) + 1 device_desc.add_channel_name(channel_name) - trace_dd("\tChannel ", i, " Name : ", channel_name.decode('utf-8')) + trace_dd("\tChannel ", i, " Name : ", channel_name.decode("utf-8")) trace_dd("unpack_device_description processed ", offset, " bytes") return offset, device_desc @@ -1064,19 +1190,24 @@ def __unpack_device_description(self, data, major, minor): def __unpack_camera_description(self, data, major, minor): offset = 0 # Name - name, separator, remainder = bytes(data[offset:]).partition(b'\0') + name, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(name) + 1 - trace_dd("\tName : %s" % name.decode('utf-8')) + trace_dd("\tName : %s" % name.decode("utf-8")) # Position - position = Vector3.unpack(data[offset:offset + 12]) + position = Vector3.unpack(data[offset : offset + 12]) offset += 12 - trace_dd("\tPosition : [%3.2f, %3.2f, %3.2f]" % (position[0], position[1], position[2])) + trace_dd( + "\tPosition : [%3.2f, %3.2f, %3.2f]" + % (position[0], position[1], position[2]) + ) # Orientation - orientation = Quaternion.unpack(data[offset:offset + 16]) + orientation = Quaternion.unpack(data[offset : offset + 16]) offset += 16 - trace_dd("\tOrientation: [%3.2f, %3.2f, %3.2f, %3.2f]" % ( - orientation[0], orientation[1], orientation[2], orientation[3])) + trace_dd( + "\tOrientation: [%3.2f, %3.2f, %3.2f, %3.2f]" + % (orientation[0], orientation[1], orientation[2], orientation[3]) + ) trace_dd("unpack_camera_description processed %3.1d bytes" % offset) camera_desc = DataDescriptions.CameraDescription(name, position, orientation) @@ -1087,36 +1218,50 @@ def __unpack_data_descriptions(self, data: bytes, packet_size, major, minor): data_descs = DataDescriptions.DataDescriptions() offset = 0 # # of data sets to process - dataset_count = int.from_bytes(data[offset:offset + 4], byteorder='little') + dataset_count = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 trace_dd("Dataset Count : ", str(dataset_count)) for i in range(0, dataset_count): trace_dd("Dataset ", str(i)) - data_type = int.from_bytes(data[offset:offset + 4], byteorder='little') + data_type = int.from_bytes(data[offset : offset + 4], byteorder="little") offset += 4 data_tmp = None if data_type == 0: trace_dd("Type: 0 Markerset") - offset_tmp, data_tmp = self.__unpack_marker_set_description(data[offset:], major, minor) + offset_tmp, data_tmp = self.__unpack_marker_set_description( + data[offset:], major, minor + ) elif data_type == 1: trace_dd("Type: 1 Rigid Body") - offset_tmp, data_tmp = self.__unpack_rigid_body_description(data[offset:], major, minor) + offset_tmp, data_tmp = self.__unpack_rigid_body_description( + data[offset:], major, minor + ) elif data_type == 2: trace_dd("Type: 2 Skeleton") - offset_tmp, data_tmp = self.__unpack_skeleton_description(data[offset:], major, minor) + offset_tmp, data_tmp = self.__unpack_skeleton_description( + data[offset:], major, minor + ) elif data_type == 3: trace_dd("Type: 3 Force Plate") - offset_tmp, data_tmp = self.__unpack_force_plate_description(data[offset:], major, minor) + offset_tmp, data_tmp = self.__unpack_force_plate_description( + data[offset:], major, minor + ) elif data_type == 4: trace_dd("Type: 4 Device") - offset_tmp, data_tmp = self.__unpack_device_description(data[offset:], major, minor) + offset_tmp, data_tmp = self.__unpack_device_description( + data[offset:], major, minor + ) elif data_type == 5: trace_dd("Type: 5 Camera") - offset_tmp, data_tmp = self.__unpack_camera_description(data[offset:], major, minor) + offset_tmp, data_tmp = self.__unpack_camera_description( + data[offset:], major, minor + ) else: print("Type: " + str(data_type) + " UNKNOWN") print("ERROR: Type decode failure") - print("\t" + str(i + 1) + " datasets processed of " + str(dataset_count)) + print( + "\t" + str(i + 1) + " datasets processed of " + str(dataset_count) + ) print("\t " + str(offset) + " bytes processed of " + str(packet_size)) print("\tPACKET DECODE STOPPED") return offset @@ -1134,11 +1279,13 @@ def __unpack_server_info(self, data, packet_size, major, minor): offset = 0 # Server name # szName = data[offset: offset+256] - self.__application_name, separator, remainder = bytes(data[offset: offset + 256]).partition(b'\0') + self.__application_name, separator, remainder = bytes( + data[offset : offset + 256] + ).partition(b"\0") self.__application_name = str(self.__application_name, "utf-8") offset += 256 # Server Version info - server_version = struct.unpack('BBBB', data[offset:offset + 4]) + server_version = struct.unpack("BBBB", data[offset : offset + 4]) offset += 4 self.__server_version[0] = server_version[0] self.__server_version[1] = server_version[1] @@ -1146,32 +1293,55 @@ def __unpack_server_info(self, data, packet_size, major, minor): self.__server_version[3] = server_version[3] # NatNet Version info - nnsvs = struct.unpack('BBBB', data[offset:offset + 4]) + nnsvs = struct.unpack("BBBB", data[offset : offset + 4]) offset += 4 self.__nat_net_stream_version_server[0] = nnsvs[0] self.__nat_net_stream_version_server[1] = nnsvs[1] self.__nat_net_stream_version_server[2] = nnsvs[2] self.__nat_net_stream_version_server[3] = nnsvs[3] - if (self.__nat_net_requested_version[0] == 0) and \ - (self.__nat_net_requested_version[1] == 0): - self.__nat_net_requested_version[0] = self.__nat_net_stream_version_server[0] - self.__nat_net_requested_version[1] = self.__nat_net_stream_version_server[1] - self.__nat_net_requested_version[2] = self.__nat_net_stream_version_server[2] - self.__nat_net_requested_version[3] = self.__nat_net_stream_version_server[3] + if (self.__nat_net_requested_version[0] == 0) and ( + self.__nat_net_requested_version[1] == 0 + ): + self.__nat_net_requested_version[0] = self.__nat_net_stream_version_server[ + 0 + ] + self.__nat_net_requested_version[1] = self.__nat_net_stream_version_server[ + 1 + ] + self.__nat_net_requested_version[2] = self.__nat_net_stream_version_server[ + 2 + ] + self.__nat_net_requested_version[3] = self.__nat_net_stream_version_server[ + 3 + ] # Determine if the bitstream version can be changed - if (self.__nat_net_stream_version_server[0] >= 4) and (self.use_multicast == False): + if (self.__nat_net_stream_version_server[0] >= 4) and ( + self.use_multicast == False + ): self.__can_change_bitstream_version = True trace_mf("Sending Application Name: ", self.__application_name) - trace_mf("NatNetVersion ", str(self.__nat_net_stream_version_server[0]), " " - , str(self.__nat_net_stream_version_server[1]), " " - , str(self.__nat_net_stream_version_server[2]), " " - , str(self.__nat_net_stream_version_server[3])) - - trace_mf("ServerVersion ", str(self.__server_version[0]), " " - , str(self.__server_version[1]), " " - , str(self.__server_version[2]), " " - , str(self.__server_version[3])) + trace_mf( + "NatNetVersion ", + str(self.__nat_net_stream_version_server[0]), + " ", + str(self.__nat_net_stream_version_server[1]), + " ", + str(self.__nat_net_stream_version_server[2]), + " ", + str(self.__nat_net_stream_version_server[3]), + ) + + trace_mf( + "ServerVersion ", + str(self.__server_version[0]), + " ", + str(self.__server_version[1]), + " ", + str(self.__server_version[2]), + " ", + str(self.__server_version[3]), + ) return offset def __command_thread_function(self, in_socket, stop, gprint_level): @@ -1190,15 +1360,17 @@ def __command_thread_function(self, in_socket, stop, gprint_level): # print("ERROR: command socket access error occurred:\n %s" %msg) # return 1 print("shutting down") - except socket.herror: + except socket.herror: print("ERROR: command socket access herror occurred") return 2 - except socket.gaierror: + except socket.gaierror: print("ERROR: command socket access gaierror occurred") return 3 - except socket.timeout: - if (self.use_multicast): - print("ERROR: command socket access timeout occurred. Server not responding") + except socket.timeout: + if self.use_multicast: + print( + "ERROR: command socket access timeout occurred. Server not responding" + ) # return 4 if len(data) > 0: @@ -1222,7 +1394,9 @@ def __command_thread_function(self, in_socket, stop, gprint_level): if not self.use_multicast: if not stop(): - self.send_keep_alive(in_socket, self.server_ip_address, self.command_port) + self.send_keep_alive( + in_socket, self.server_ip_address, self.command_port + ) return 0 def __data_thread_function(self, in_socket, stop, gprint_level): @@ -1239,15 +1413,17 @@ def __data_thread_function(self, in_socket, stop, gprint_level): if not stop(): print("ERROR: data socket access error occurred:\n %s" % msg) return 1 - except socket.herror: + except socket.herror: print("ERROR: data socket access herror occurred") # return 2 - except socket.gaierror: + except socket.gaierror: print("ERROR: data socket access gaierror occurred") # return 3 - except socket.timeout: + except socket.timeout: # if self.use_multicast: - print("ERROR: data socket access timeout occurred. Server not responding") + print( + "ERROR: data socket access timeout occurred. Server not responding" + ) # return 4 if len(data) > 0: # peek ahead at message_id @@ -1277,14 +1453,20 @@ def __process_message(self, data: bytes, print_level=0): trace("Begin Packet\n-----------------") show_nat_net_version = False if show_nat_net_version: - trace("NatNetVersion ", str(self.__nat_net_requested_version[0]), " " \ - , str(self.__nat_net_requested_version[1]), " " \ - , str(self.__nat_net_requested_version[2]), " " \ - , str(self.__nat_net_requested_version[3])) + trace( + "NatNetVersion ", + str(self.__nat_net_requested_version[0]), + " ", + str(self.__nat_net_requested_version[1]), + " ", + str(self.__nat_net_requested_version[2]), + " ", + str(self.__nat_net_requested_version[3]), + ) message_id = get_message_id(data) - packet_size = int.from_bytes(data[2:4], byteorder='little') + packet_size = int.from_bytes(data[2:4], byteorder="little") # skip the 4 bytes for message ID and packet_size offset = 4 @@ -1292,7 +1474,9 @@ def __process_message(self, data: bytes, print_level=0): trace("Message ID : %3.1d NAT_FRAMEOFDATA" % message_id) trace("Packet Size : ", packet_size) - offset_tmp, mocap_data = self.__unpack_mocap_data(data[offset:], packet_size, major, minor) + offset_tmp, mocap_data = self.__unpack_mocap_data( + data[offset:], packet_size, major, minor + ) offset += offset_tmp print("MoCap Frame: %d\n" % (mocap_data.prefix_data.frame_number)) # get a string version of the data for output @@ -1303,7 +1487,9 @@ def __process_message(self, data: bytes, print_level=0): elif message_id == self.NAT_MODELDEF: trace("Message ID : %3.1d NAT_MODELDEF" % message_id) trace("Packet Size : %d" % packet_size) - offset_tmp, data_descs = self.__unpack_data_descriptions(data[offset:], packet_size, major, minor) + offset_tmp, data_descs = self.__unpack_data_descriptions( + data[offset:], packet_size, major, minor + ) offset += offset_tmp print("Data Descriptions:\n") # get a string version of the data for output @@ -1314,24 +1500,34 @@ def __process_message(self, data: bytes, print_level=0): elif message_id == self.NAT_SERVERINFO: trace("Message ID : %3.1d NAT_SERVERINFO" % message_id) trace("Packet Size : ", packet_size) - offset += self.__unpack_server_info(data[offset:], packet_size, major, minor) + offset += self.__unpack_server_info( + data[offset:], packet_size, major, minor + ) elif message_id == self.NAT_RESPONSE: trace("Message ID : %3.1d NAT_RESPONSE" % message_id) trace("Packet Size : ", packet_size) if packet_size == 4: - command_response = int.from_bytes(data[offset:offset + 4], byteorder='little') + command_response = int.from_bytes( + data[offset : offset + 4], byteorder="little" + ) offset += 4 trace("Command response: %d" % command_response) else: show_remainder = False - message, separator, remainder = bytes(data[offset:]).partition(b'\0') + message, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(message) + 1 - if (show_remainder): - trace("Command response:", message.decode('utf-8'), \ - " separator:", separator, " remainder:", remainder) + if show_remainder: + trace( + "Command response:", + message.decode("utf-8"), + " separator:", + separator, + " remainder:", + remainder, + ) else: - trace("Command response:", message.decode('utf-8')) + trace("Command response:", message.decode("utf-8")) elif message_id == self.NAT_UNRECOGNIZED_REQUEST: trace("Message ID : %3.1d NAT_UNRECOGNIZED_REQUEST: " % message_id) trace("Packet Size : ", packet_size) @@ -1339,9 +1535,9 @@ def __process_message(self, data: bytes, print_level=0): elif message_id == self.NAT_MESSAGESTRING: trace("Message ID : %3.1d NAT_MESSAGESTRING" % message_id) trace("Packet Size : ", packet_size) - message, separator, remainder = bytes(data[offset:]).partition(b'\0') + message, separator, remainder = bytes(data[offset:]).partition(b"\0") offset += len(message) + 1 - trace("Received message from server:", message.decode('utf-8')) + trace("Received message from server:", message.decode("utf-8")) else: trace("Message ID : %3.1d UNKNOWN" % message_id) trace("Packet Size : ", packet_size) @@ -1353,7 +1549,10 @@ def __process_message(self, data: bytes, print_level=0): def send_request(self, in_socket, command, command_str, address): # Compose the message in our known message format packet_size = 0 - if command == self.NAT_REQUEST_MODELDEF or command == self.NAT_REQUEST_FRAMEOFDATA: + if ( + command == self.NAT_REQUEST_MODELDEF + or command == self.NAT_REQUEST_FRAMEOFDATA + ): packet_size = 0 command_str = "" elif command == self.NAT_REQUEST: @@ -1365,11 +1564,11 @@ def send_request(self, in_socket, command, command_str, address): packet_size = 0 command_str = "" - data = command.to_bytes(2, byteorder='little') - data += packet_size.to_bytes(2, byteorder='little') + data = command.to_bytes(2, byteorder="little") + data += packet_size.to_bytes(2, byteorder="little") - data += command_str.encode('utf-8') - data += b'\0' + data += command_str.encode("utf-8") + data += b"\0" return in_socket.sendto(data, address) @@ -1378,10 +1577,14 @@ def send_command(self, command_str): ret_val = -1 while nTries: nTries -= 1 - ret_val = self.send_request(self.command_socket, self.NAT_REQUEST, command_str, - (self.server_ip_address, self.command_port)) - if (ret_val != -1): - break; + ret_val = self.send_request( + self.command_socket, + self.NAT_REQUEST, + command_str, + (self.server_ip_address, self.command_port), + ) + if ret_val != -1: + break return ret_val # return self.send_request(self.data_socket, self.NAT_REQUEST, command_str, (self.server_ip_address, self.command_port) ) @@ -1389,11 +1592,13 @@ def send_command(self, command_str): def send_commands(self, tmpCommands, print_results: bool = True): for sz_command in tmpCommands: return_code = self.send_command(sz_command) - if (print_results): + if print_results: print("Command: %s - return_code: %d" % (sz_command, return_code)) def send_keep_alive(self, in_socket, server_ip_address, server_port): - return self.send_request(in_socket, self.NAT_KEEPALIVE, "", (server_ip_address, server_port)) + return self.send_request( + in_socket, self.NAT_KEEPALIVE, "", (server_ip_address, server_port) + ) def get_command_port(self): return self.command_port @@ -1431,18 +1636,35 @@ def run(self): self.stop_threads = False # Create a separate thread for receiving data packets - self.data_thread = Thread(target=self.__data_thread_function, - args=(self.data_socket, lambda: self.stop_threads, lambda: self.print_level,)) + self.data_thread = Thread( + target=self.__data_thread_function, + args=( + self.data_socket, + lambda: self.stop_threads, + lambda: self.print_level, + ), + ) self.data_thread.start() # Create a separate thread for receiving command packets - self.command_thread = Thread(target=self.__command_thread_function, - args=(self.command_socket, lambda: self.stop_threads, lambda: self.print_level,)) + self.command_thread = Thread( + target=self.__command_thread_function, + args=( + self.command_socket, + lambda: self.stop_threads, + lambda: self.print_level, + ), + ) self.command_thread.start() # Required for setup # Get NatNet and server versions - self.send_request(self.command_socket, self.NAT_CONNECT, "", (self.server_ip_address, self.command_port)) + self.send_request( + self.command_socket, + self.NAT_CONNECT, + "", + (self.server_ip_address, self.command_port), + ) ##Example Commands ## Get NatNet and server versions @@ -1477,46 +1699,57 @@ def unpack_mocap_data(self, data: bytes, packet_size, major, minor): frame_number = frame_prefix_data.frame_number # Marker Set Data - rel_offset, marker_set_data = self.__unpack_marker_set_data(data[offset:], (packet_size - offset), major, minor) + rel_offset, marker_set_data = self.__unpack_marker_set_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_marker_set_data(marker_set_data) marker_set_count = marker_set_data.get_marker_set_count() unlabeled_markers_count = marker_set_data.get_unlabeled_marker_count() # Rigid Body Data - rel_offset, rigid_body_data = self.__unpack_rigid_body_data(data[offset:], (packet_size - offset), major, minor) + rel_offset, rigid_body_data = self.__unpack_rigid_body_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_rigid_body_data(rigid_body_data) rigid_body_count = rigid_body_data.get_rigid_body_count() # Skeleton Data - rel_offset, skeleton_data = self.__unpack_skeleton_data(data[offset:], (packet_size - offset), major, minor) + rel_offset, skeleton_data = self.__unpack_skeleton_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_skeleton_data(skeleton_data) skeleton_count = skeleton_data.get_skeleton_count() # Labeled Marker Data - rel_offset, labeled_marker_data = self.__unpack_labeled_marker_data(data[offset:], (packet_size - offset), - major, minor) + rel_offset, labeled_marker_data = self.__unpack_labeled_marker_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_labeled_marker_data(labeled_marker_data) labeled_marker_count = labeled_marker_data.get_labeled_marker_count() # Force Plate Data - rel_offset, force_plate_data = self.__unpack_force_plate_data(data[offset:], (packet_size - offset), major, - minor) + rel_offset, force_plate_data = self.__unpack_force_plate_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_force_plate_data(force_plate_data) # Device Data - rel_offset, device_data = self.__unpack_device_data(data[offset:], (packet_size - offset), major, minor) + rel_offset, device_data = self.__unpack_device_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_device_data(device_data) # Frame Suffix Data # rel_offset, timecode, timecode_sub, timestamp, is_recording, tracked_models_changed = \ - rel_offset, frame_suffix_data = self.__unpack_frame_suffix_data(data[offset:], (packet_size - offset), major, - minor) + rel_offset, frame_suffix_data = self.__unpack_frame_suffix_data( + data[offset:], (packet_size - offset), major, minor + ) offset += rel_offset mocap_data.set_suffix_data(frame_suffix_data) diff --git a/src/rotools/optitrack/windows/PythonSample.py b/src/rotools/optitrack/windows/PythonSample.py index a4bec03..cbca655 100755 --- a/src/rotools/optitrack/windows/PythonSample.py +++ b/src/rotools/optitrack/windows/PythonSample.py @@ -28,8 +28,19 @@ # This is a callback function that gets connected to the NatNet client # and called once per mocap frame. def receive_new_frame(data_dict): - order_list = ["frameNumber", "markerSetCount", "unlabeledMarkersCount", "rigidBodyCount", "skeletonCount", - "labeledMarkerCount", "timecode", "timecodeSub", "timestamp", "isRecording", "trackedMdelsChangedo"] + order_list = [ + "frameNumber", + "markerSetCount", + "unlabeledMarkersCount", + "rigidBodyCount", + "skeletonCount", + "labeledMarkerCount", + "timecode", + "timecodeSub", + "timestamp", + "isRecording", + "trackedMdelsChangedo", + ] dump_args = False if dump_args == True: out_string = " " @@ -76,13 +87,29 @@ def print_configuration(natnet_client): print(" NatNet Server Info") print(" Application Name %s" % (application_name)) - print(" NatNetVersion %d %d %d %d" % ( - nat_net_version_server[0], nat_net_version_server[1], nat_net_version_server[2], nat_net_version_server[3])) print( - " ServerVersion %d %d %d %d" % (server_version[0], server_version[1], server_version[2], server_version[3])) + " NatNetVersion %d %d %d %d" + % ( + nat_net_version_server[0], + nat_net_version_server[1], + nat_net_version_server[2], + nat_net_version_server[3], + ) + ) + print( + " ServerVersion %d %d %d %d" + % (server_version[0], server_version[1], server_version[2], server_version[3]) + ) print(" NatNet Bitstream Requested") - print(" NatNetVersion %d %d %d %d" % (nat_net_requested_version[0], nat_net_requested_version[1], \ - nat_net_requested_version[2], nat_net_requested_version[3])) + print( + " NatNetVersion %d %d %d %d" + % ( + nat_net_requested_version[0], + nat_net_requested_version[1], + nat_net_requested_version[2], + nat_net_requested_version[3], + ) + ) # print("command_socket = %s"%(str(natnet_client.command_socket))) # print("data_socket = %s"%(str(natnet_client.data_socket))) @@ -96,12 +123,16 @@ def print_commands(can_change_bitstream): outstring += " pause may require several seconds\n" outstring += " depending on the frame data size\n" outstring += "Change Working Range\n" - outstring += " o reset Working Range to: start/current/end frame = 0/0/end of take\n" + outstring += ( + " o reset Working Range to: start/current/end frame = 0/0/end of take\n" + ) outstring += " w set Working Range to: start/current/end frame = 1/100/1500\n" outstring += "Return Data Display Modes\n" outstring += " j print_level = 0 supress data description and mocap frame data\n" outstring += " k print_level = 1 show data description and mocap frame data\n" - outstring += " l print_level = 20 show data description and every 20th mocap frame data\n" + outstring += ( + " l print_level = 20 show data description and every 20th mocap frame data\n" + ) outstring += "Change NatNet data stream version (Unicast only)\n" outstring += " 3 Request 3.1 data stream (Unicast only)\n" outstring += " 4 Request 4.0 data stream (Unicast only)\n" @@ -114,16 +145,20 @@ def print_commands(can_change_bitstream): outstring += " Endpoint, Loop, and Bounce playback modes.\n" outstring += "\n" outstring += "EXAMPLE: PacketClient [serverIP [ clientIP [ Multicast/Unicast]]]\n" - outstring += " PacketClient \"192.168.10.14\" \"192.168.10.14\" Multicast\n" - outstring += " PacketClient \"127.0.0.1\" \"127.0.0.1\" u\n" + outstring += ' PacketClient "192.168.10.14" "192.168.10.14" Multicast\n' + outstring += ' PacketClient "127.0.0.1" "127.0.0.1" u\n' outstring += "\n" print(outstring) def request_data_descriptions(s_client): # Request the model definitions - s_client.send_request(s_client.command_socket, s_client.NAT_REQUEST_MODELDEF, "", - (s_client.server_ip_address, s_client.command_port)) + s_client.send_request( + s_client.command_socket, + s_client.NAT_REQUEST_MODELDEF, + "", + (s_client.server_ip_address, s_client.command_port), + ) def test_classes(): @@ -207,92 +242,105 @@ def my_parse_args(arg_list, args_dict): print_commands(streaming_client.can_change_bitstream_version()) while is_looping: - inchars = input('Enter command or (\'h\' for list of commands)\n') + inchars = input("Enter command or ('h' for list of commands)\n") if len(inchars) > 0: c1 = inchars[0].lower() - if c1 == 'h': + if c1 == "h": print_commands(streaming_client.can_change_bitstream_version()) - elif c1 == 'c': + elif c1 == "c": print_configuration(streaming_client) - elif c1 == 's': + elif c1 == "s": request_data_descriptions(streaming_client) time.sleep(1) - elif (c1 == '3') or (c1 == '4'): + elif (c1 == "3") or (c1 == "4"): if streaming_client.can_change_bitstream_version(): tmp_major = 4 tmp_minor = 0 - if (c1 == '3'): + if c1 == "3": tmp_major = 3 tmp_minor = 1 - return_code = streaming_client.set_nat_net_version(tmp_major, tmp_minor) + return_code = streaming_client.set_nat_net_version( + tmp_major, tmp_minor + ) time.sleep(1) if return_code == -1: - print("Could not change bitstream version to %d.%d" % (tmp_major, tmp_minor)) + print( + "Could not change bitstream version to %d.%d" + % (tmp_major, tmp_minor) + ) else: print("Bitstream version at %d.%d" % (tmp_major, tmp_minor)) else: print("Can only change bitstream in Unicast Mode") - elif c1 == 'p': + elif c1 == "p": sz_command = "TimelineStop" return_code = streaming_client.send_command(sz_command) time.sleep(1) print("Command: %s - return_code: %d" % (sz_command, return_code)) - elif c1 == 'r': + elif c1 == "r": sz_command = "TimelinePlay" return_code = streaming_client.send_command(sz_command) print("Command: %s - return_code: %d" % (sz_command, return_code)) - elif c1 == 'o': - tmpCommands = ["TimelinePlay", - "TimelineStop", - "SetPlaybackStartFrame,0", - "SetPlaybackStopFrame,1000000", - "SetPlaybackLooping,0", - "SetPlaybackCurrentFrame,0", - "TimelineStop"] + elif c1 == "o": + tmpCommands = [ + "TimelinePlay", + "TimelineStop", + "SetPlaybackStartFrame,0", + "SetPlaybackStopFrame,1000000", + "SetPlaybackLooping,0", + "SetPlaybackCurrentFrame,0", + "TimelineStop", + ] for sz_command in tmpCommands: return_code = streaming_client.send_command(sz_command) print("Command: %s - return_code: %d" % (sz_command, return_code)) time.sleep(1) - elif c1 == 'w': - tmp_commands = ["TimelinePlay", - "TimelineStop", - "SetPlaybackStartFrame,10", - "SetPlaybackStopFrame,1500", - "SetPlaybackLooping,0", - "SetPlaybackCurrentFrame,100", - "TimelineStop"] + elif c1 == "w": + tmp_commands = [ + "TimelinePlay", + "TimelineStop", + "SetPlaybackStartFrame,10", + "SetPlaybackStopFrame,1500", + "SetPlaybackLooping,0", + "SetPlaybackCurrentFrame,100", + "TimelineStop", + ] for sz_command in tmp_commands: return_code = streaming_client.send_command(sz_command) print("Command: %s - return_code: %d" % (sz_command, return_code)) time.sleep(1) - elif c1 == 't': + elif c1 == "t": test_classes() - elif c1 == 'j': + elif c1 == "j": streaming_client.set_print_level(0) - print("Showing only received frame numbers and supressing data descriptions") - elif c1 == 'k': + print( + "Showing only received frame numbers and supressing data descriptions" + ) + elif c1 == "k": streaming_client.set_print_level(1) print("Showing every received frame") - elif c1 == 'l': + elif c1 == "l": print_level = streaming_client.set_print_level(20) print_level_mod = print_level % 100 - if (print_level == 0): - print("Showing only received frame numbers and supressing data descriptions") - elif (print_level == 1): + if print_level == 0: + print( + "Showing only received frame numbers and supressing data descriptions" + ) + elif print_level == 1: print("Showing every frame") - elif (print_level_mod == 1): + elif print_level_mod == 1: print("Showing every %dst frame" % print_level) - elif (print_level_mod == 2): + elif print_level_mod == 2: print("Showing every %dnd frame" % print_level) - elif (print_level == 3): + elif print_level == 3: print("Showing every %drd frame" % print_level) else: print("Showing every %dth frame" % print_level) - elif c1 == 'q': + elif c1 == "q": is_looping = False streaming_client.shutdown() break diff --git a/src/rotools/optitrack/windows/start_server.py b/src/rotools/optitrack/windows/start_server.py index 61a64dd..b4a97f1 100644 --- a/src/rotools/optitrack/windows/start_server.py +++ b/src/rotools/optitrack/windows/start_server.py @@ -54,36 +54,44 @@ def receive_from_optitrack(rbd_dict, connection): while True: in_socket = connection.data_socket data, addr = in_socket.recvfrom(recv_buffer_size) - packet_size = int.from_bytes(data[2:4], byteorder='little') - offset_tmp, mocap_data = connection.unpack_mocap_data(data[offset:], packet_size, major, minor) + packet_size = int.from_bytes(data[2:4], byteorder="little") + offset_tmp, mocap_data = connection.unpack_mocap_data( + data[offset:], packet_size, major, minor + ) data_rbd = mocap_data.rigid_body_data data_rbd = data_rbd.get_as_string() - data_rbd = data_rbd.encode('utf-8') - rbd_dict['data_rbd'] = data_rbd + data_rbd = data_rbd.encode("utf-8") + rbd_dict["data_rbd"] = data_rbd def send_to_ubuntu(server, rbd_dict, connection): while True: try: - data_rbd = rbd_dict['data_rbd'] + data_rbd = rbd_dict["data_rbd"] connection.sendall(data_rbd) rec = connection.recv(64) except KeyError: - print('Rigid body dict has no data_rbd key, maybe optitrack stream is not alive') - time.sleep(1.) + print( + "Rigid body dict has no data_rbd key, maybe optitrack stream is not alive" + ) + time.sleep(1.0) except ConnectionError: - print('The connection has lost, waiting for reconnection ...') + print("The connection has lost, waiting for reconnection ...") connection, (host, port) = server.accept() if __name__ == "__main__": server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - server.bind(('192.168.13.118', 6688)) + server.bind(("192.168.13.118", 6688)) server.listen(5) connect, (host, port) = server.accept() - print(u'the client %s:%s has connected.' % (host, port)) + print("the client %s:%s has connected." % (host, port)) - optionsDict = {"clientAddress": "127.0.0.1", "serverAddress": "127.0.0.1", "use_multicast": True} + optionsDict = { + "clientAddress": "127.0.0.1", + "serverAddress": "127.0.0.1", + "use_multicast": True, + } # This will create a new NatNet client optionsDict = my_parse_args(sys.argv, optionsDict) @@ -111,8 +119,21 @@ def send_to_ubuntu(server, rbd_dict, connection): with Manager() as manager: dict_share = manager.dict() - optitrack_stream_process = Process(target=receive_from_optitrack, args=(dict_share, streaming_client,)) - send2client_process = Process(target=send_to_ubuntu, args=(server, dict_share, connect,)) + optitrack_stream_process = Process( + target=receive_from_optitrack, + args=( + dict_share, + streaming_client, + ), + ) + send2client_process = Process( + target=send_to_ubuntu, + args=( + server, + dict_share, + connect, + ), + ) optitrack_stream_process.start() send2client_process.start() optitrack_stream_process.join() diff --git a/src/rotools/optitrack/windows/test_api.py b/src/rotools/optitrack/windows/test_api.py index 693d0b1..4d879cf 100755 --- a/src/rotools/optitrack/windows/test_api.py +++ b/src/rotools/optitrack/windows/test_api.py @@ -28,8 +28,19 @@ # This is a callback function that gets connected to the NatNet client # and called once per mocap frame. def receive_new_frame(data_dict): - order_list = ["frameNumber", "markerSetCount", "unlabeledMarkersCount", "rigidBodyCount", "skeletonCount", - "labeledMarkerCount", "timecode", "timecodeSub", "timestamp", "isRecording", "trackedMdelsChangedo"] + order_list = [ + "frameNumber", + "markerSetCount", + "unlabeledMarkersCount", + "rigidBodyCount", + "skeletonCount", + "labeledMarkerCount", + "timecode", + "timecodeSub", + "timestamp", + "isRecording", + "trackedMdelsChangedo", + ] dump_args = False if dump_args == True: out_string = " " @@ -76,13 +87,29 @@ def print_configuration(natnet_client): print(" NatNet Server Info") print(" Application Name %s" % (application_name)) - print(" NatNetVersion %d %d %d %d" % ( - nat_net_version_server[0], nat_net_version_server[1], nat_net_version_server[2], nat_net_version_server[3])) print( - " ServerVersion %d %d %d %d" % (server_version[0], server_version[1], server_version[2], server_version[3])) + " NatNetVersion %d %d %d %d" + % ( + nat_net_version_server[0], + nat_net_version_server[1], + nat_net_version_server[2], + nat_net_version_server[3], + ) + ) + print( + " ServerVersion %d %d %d %d" + % (server_version[0], server_version[1], server_version[2], server_version[3]) + ) print(" NatNet Bitstream Requested") - print(" NatNetVersion %d %d %d %d" % (nat_net_requested_version[0], nat_net_requested_version[1], \ - nat_net_requested_version[2], nat_net_requested_version[3])) + print( + " NatNetVersion %d %d %d %d" + % ( + nat_net_requested_version[0], + nat_net_requested_version[1], + nat_net_requested_version[2], + nat_net_requested_version[3], + ) + ) # print("command_socket = %s"%(str(natnet_client.command_socket))) # print("data_socket = %s"%(str(natnet_client.data_socket))) @@ -96,12 +123,16 @@ def print_commands(can_change_bitstream): outstring += " pause may require several seconds\n" outstring += " depending on the frame data size\n" outstring += "Change Working Range\n" - outstring += " o reset Working Range to: start/current/end frame = 0/0/end of take\n" + outstring += ( + " o reset Working Range to: start/current/end frame = 0/0/end of take\n" + ) outstring += " w set Working Range to: start/current/end frame = 1/100/1500\n" outstring += "Return Data Display Modes\n" outstring += " j print_level = 0 supress data description and mocap frame data\n" outstring += " k print_level = 1 show data description and mocap frame data\n" - outstring += " l print_level = 20 show data description and every 20th mocap frame data\n" + outstring += ( + " l print_level = 20 show data description and every 20th mocap frame data\n" + ) outstring += "Change NatNet data stream version (Unicast only)\n" outstring += " 3 Request 3.1 data stream (Unicast only)\n" outstring += " 4 Request 4.0 data stream (Unicast only)\n" @@ -114,16 +145,20 @@ def print_commands(can_change_bitstream): outstring += " Endpoint, Loop, and Bounce playback modes.\n" outstring += "\n" outstring += "EXAMPLE: PacketClient [serverIP [ clientIP [ Multicast/Unicast]]]\n" - outstring += " PacketClient \"192.168.10.14\" \"192.168.10.14\" Multicast\n" - outstring += " PacketClient \"127.0.0.1\" \"127.0.0.1\" u\n" + outstring += ' PacketClient "192.168.10.14" "192.168.10.14" Multicast\n' + outstring += ' PacketClient "127.0.0.1" "127.0.0.1" u\n' outstring += "\n" print(outstring) def request_data_descriptions(s_client): # Request the model definitions - s_client.send_request(s_client.command_socket, s_client.NAT_REQUEST_MODELDEF, "", - (s_client.server_ip_address, s_client.command_port)) + s_client.send_request( + s_client.command_socket, + s_client.NAT_REQUEST_MODELDEF, + "", + (s_client.server_ip_address, s_client.command_port), + ) def test_classes(): @@ -215,8 +250,10 @@ def my_parse_args(arg_list, args_dict): in_socket = streaming_client.data_socket data, addr = in_socket.recvfrom(recv_buffer_size) - packet_size = int.from_bytes(data[2:4], byteorder='little') - offset_tmp, mocap_data = streaming_client.unpack_mocap_data(data[offset:], packet_size, major, minor) + packet_size = int.from_bytes(data[2:4], byteorder="little") + offset_tmp, mocap_data = streaming_client.unpack_mocap_data( + data[offset:], packet_size, major, minor + ) # streaming_client.send_request(streaming_client.command_socket, streaming_client.NAT_CONNECT, "", (streaming_client.server_ip_address, streaming_client.command_port)) data_p = mocap_data.rigid_body_data data_p = data_p.get_as_string() diff --git a/src/rotools/optitrack/windows/test_socket.py b/src/rotools/optitrack/windows/test_socket.py index 0f1f0a0..695dc16 100755 --- a/src/rotools/optitrack/windows/test_socket.py +++ b/src/rotools/optitrack/windows/test_socket.py @@ -1,12 +1,12 @@ import socket + server = socket.socket(socket.AF_INET, socket.SOCK_STREAM) -server.bind(('192.168.13.118', 6688)) +server.bind(("192.168.13.118", 6688)) server.listen(5) -print(u'waiting for connect...') +print("waiting for connect...") connect, (host, port) = server.accept() -print(u'the client %s:%s has connected.' % (host, port)) +print("the client %s:%s has connected." % (host, port)) while True: - connect.sendall(b'your words has received.') - + connect.sendall(b"your words has received.") diff --git a/src/rotools/planner/core/interface.py b/src/rotools/planner/core/interface.py index e00210d..72cf9ca 100644 --- a/src/rotools/planner/core/interface.py +++ b/src/rotools/planner/core/interface.py @@ -6,16 +6,10 @@ class PlannerInterface(object): - """ - - """ + """ """ def __init__( - self, - algorithm_port, - robot_initial_poses, - planning_initial_poses, - **kwargs + self, algorithm_port, robot_initial_poses, planning_initial_poses, **kwargs ): """ :param algorithm_port: str The port of the algorithm server. It takes the form: @@ -26,15 +20,17 @@ def __init__( """ super(PlannerInterface, self).__init__() - if not isinstance(algorithm_port, str) or algorithm_port == '': - rospy.logerr('algorithm_port is not defined: {}'.format(algorithm_port)) + if not isinstance(algorithm_port, str) or algorithm_port == "": + rospy.logerr("algorithm_port is not defined: {}".format(algorithm_port)) self._algorithm_port = algorithm_port robot_initial_poses = np.array(robot_initial_poses) planning_initial_poses = np.array(planning_initial_poses) - assert robot_initial_poses.shape[-1] == 7 and planning_initial_poses.shape == robot_initial_poses.shape, \ - rospy.logerr('poses shape should be (N, 7)') + assert ( + robot_initial_poses.shape[-1] == 7 + and planning_initial_poses.shape == robot_initial_poses.shape + ), rospy.logerr("poses shape should be (N, 7)") self._robot_initial_poses = robot_initial_poses self._planning_initial_poses = planning_initial_poses @@ -58,7 +54,9 @@ def set_initial_translations(self): """ translations = [] - for r_pose, p_pose in zip(self._robot_initial_poses, self._planning_initial_poses): + for r_pose, p_pose in zip( + self._robot_initial_poses, self._planning_initial_poses + ): translations.append(common.get_transform_same_target(r_pose, p_pose)) return translations @@ -68,11 +66,15 @@ def get_plan(self): return False, None out_list = [] for poses in trajectories: - assert len(poses) == len(self._translations), rospy.logerr('Poses number is not equal to translations') + assert len(poses) == len(self._translations), rospy.logerr( + "Poses number is not equal to translations" + ) pose_msg_list = [] for i, pose in enumerate(poses): std_pose = common.sd_pose(pose) # pose in the planning frame - std_robot_pose = np.dot(self._translations[i], std_pose) # pose in the robot frame + std_robot_pose = np.dot( + self._translations[i], std_pose + ) # pose in the robot frame pose_stamped_msg = common.to_ros_pose_stamped(std_robot_pose) pose_msg_list.append(pose_stamped_msg) out_list.append(pose_msg_list) @@ -90,18 +92,20 @@ def _post_data(self): relative to the planning start point (set at the planner side, not here) """ header = {} - body = {'data': json.dumps(self._planning_initial_poses.tolist())} - payload = {'header': header, 'body': body} + body = {"data": json.dumps(self._planning_initial_poses.tolist())} + payload = {"header": header, "body": body} - feedback = common.post_http_requests('http://{}'.format(self._algorithm_port), payload=payload) + feedback = common.post_http_requests( + "http://{}".format(self._algorithm_port), payload=payload + ) if feedback is None: return False, None # print(feedback) results = feedback.json() # The keys 'status' and 'results' should be coincide with the definition in the Flask server - ok = results['response']['status'] + ok = results["response"]["status"] if ok: - return True, json.loads(results['response']['results']) + return True, json.loads(results["response"]["results"]) else: - print('Post data failed to load results') + print("Post data failed to load results") return False, None diff --git a/src/rotools/planner/core/server.py b/src/rotools/planner/core/server.py index 40a406c..5133610 100644 --- a/src/rotools/planner/core/server.py +++ b/src/rotools/planner/core/server.py @@ -19,25 +19,33 @@ class PlannerServer(EStop): """ def __init__(self, kwargs): - super(PlannerServer, self).__init__(function_name='Planner execution') - modes = {0: 'step once', 1: 'loop once', 2: 'infinite loop'} - - mode = kwargs['mode'] - assert mode in [0, 1, 2], rospy.logerr('Mode should be 0 (step once), 1 (loop once), or 2 (infinite loop),' - 'but {} was given'.format(mode)) + super(PlannerServer, self).__init__(function_name="Planner execution") + modes = {0: "step once", 1: "loop once", 2: "infinite loop"} + + mode = kwargs["mode"] + assert mode in [0, 1, 2], rospy.logerr( + "Mode should be 0 (step once), 1 (loop once), or 2 (infinite loop)," + "but {} was given".format(mode) + ) self._mode = mode - rospy.loginfo('Planner server running in mode: {}'.format(modes[self._mode])) + rospy.loginfo("Planner server running in mode: {}".format(modes[self._mode])) # Publisher switch - self.srv_pub_switch = rospy.Service('/planner/enable', SetBool, self.pub_switch_handle) + self.srv_pub_switch = rospy.Service( + "/planner/enable", SetBool, self.pub_switch_handle + ) - self.srv_initialize = rospy.Service('/planner/initialize', Trigger, self.initialize_handle) + self.srv_initialize = rospy.Service( + "/planner/initialize", Trigger, self.initialize_handle + ) self.interface = interface.PlannerInterface(**kwargs) # Cartesian pose publishers - control_topics = kwargs['control_topics'] - assert isinstance(control_topics, list), rospy.logerr('control_topics is not a list') + control_topics = kwargs["control_topics"] + assert isinstance(control_topics, list), rospy.logerr( + "control_topics is not a list" + ) self._control_publishers = [] for topic in control_topics: publisher = rospy.Publisher(topic, PoseStamped, queue_size=1) @@ -45,24 +53,26 @@ def __init__(self, kwargs): self._n_topics = len(self._control_publishers) self._pose_initialized = False - self.rate = kwargs['rate'] + self.rate = kwargs["rate"] # Planner query rate is set as 100 Hz, note that this is not equal to the actual execution rate - self.plan_execution_timer = rospy.Timer(rospy.Duration.from_sec(0.01), self.execute_plan_handle) + self.plan_execution_timer = rospy.Timer( + rospy.Duration.from_sec(0.01), self.execute_plan_handle + ) def pub_switch_handle(self, req): if req.data: self.set_status(True) - msg = 'Plan execution enabled' + msg = "Plan execution enabled" else: self.set_status(False) - msg = 'Plan execution disabled' + msg = "Plan execution disabled" play_hint_sound(req.data) return SetBoolResponse(True, msg) def initialize_handle(self, req): self._initialize() - rospy.loginfo('Initial poses of the robot have been published.') - return TriggerResponse(True, 'Initialized') + rospy.loginfo("Initial poses of the robot have been published.") + return TriggerResponse(True, "Initialized") def _initialize(self): initial_poses = self.interface.get_robot_initial_poses() @@ -74,7 +84,9 @@ def execute_plan_handle(self, event): if not self.enabled: return if not self._pose_initialized: - rospy.logwarn_throttle(3, 'Poses not initialized, call the service /planner/initialize first') + rospy.logwarn_throttle( + 3, "Poses not initialized, call the service /planner/initialize first" + ) return ok, trajectories = self.interface.get_plan() if ok: @@ -87,7 +99,7 @@ def execute_plan_handle(self, event): for target_poses in trajectories: if self.enabled: self._publish_poses(target_poses) - rospy.sleep(rospy.Duration.from_sec(1. / self.rate)) + rospy.sleep(rospy.Duration.from_sec(1.0 / self.rate)) self.set_status(False) self._pose_initialized = False else: @@ -95,22 +107,25 @@ def execute_plan_handle(self, event): while self.enabled: target_poses = trajectories[cnt] self._publish_poses(target_poses) - rospy.sleep(rospy.Duration.from_sec(1. / self.rate)) + rospy.sleep(rospy.Duration.from_sec(1.0 / self.rate)) cnt += 1 if cnt >= len(trajectories): self._initialize() - rospy.sleep(rospy.Duration.from_sec(1. / self.rate)) + rospy.sleep(rospy.Duration.from_sec(1.0 / self.rate)) cnt = 0 self._pose_initialized = False else: - rospy.logwarn_throttle(3, 'Cannot get a plan') + rospy.logwarn_throttle(3, "Cannot get a plan") def _publish_poses(self, poses): - """The poses order should be semantically the same as the publishers. - - """ - assert len(poses) == self._n_topics, \ - rospy.logerr('Target poses {} and publisher number {} mismatch'.format(len(poses), self._n_topics)) + """The poses order should be semantically the same as the publishers.""" + assert len(poses) == self._n_topics, rospy.logerr( + "Target poses {} and publisher number {} mismatch".format( + len(poses), self._n_topics + ) + ) for i, pose in enumerate(poses): - assert isinstance(pose, PoseStamped), rospy.logerr('Target pose type is not PoseStamped') + assert isinstance(pose, PoseStamped), rospy.logerr( + "Target pose type is not PoseStamped" + ) self._control_publishers[i].publish(pose) diff --git a/src/rotools/policy/dmp/interface.py b/src/rotools/policy/dmp/interface.py index 69e369f..24c95d1 100644 --- a/src/rotools/policy/dmp/interface.py +++ b/src/rotools/policy/dmp/interface.py @@ -6,9 +6,7 @@ class DMPInterface(object): - """ - - """ + """ """ def __init__(self, n_dim, k, **kwargs): """ @@ -23,9 +21,11 @@ def __init__(self, n_dim, k, **kwargs): self.dmp_list = None self.tau = None - self._set_client = rospy.ServiceProxy('/set_active_dmp', SetActiveDMP) - self._learning_client = rospy.ServiceProxy('/learn_dmp_from_demo', LearnDMPFromDemo) - self._get_plan_client = rospy.ServiceProxy('/get_dmp_plan', GetDMPPlan) + self._set_client = rospy.ServiceProxy("/set_active_dmp", SetActiveDMP) + self._learning_client = rospy.ServiceProxy( + "/learn_dmp_from_demo", LearnDMPFromDemo + ) + self._get_plan_client = rospy.ServiceProxy("/get_dmp_plan", GetDMPPlan) def learning(self, demo): """ @@ -35,14 +35,17 @@ def learning(self, demo): """ req = LearnDMPFromDemoRequest() dmp_trajectory = DMPTraj() - points = demo['points'] + points = demo["points"] for point in points: dmp_point = DMPPoint() dmp_point.positions = point[0] dmp_point.velocities = point[1] - assert len(dmp_point.positions) == self.n_dim and len(dmp_point.velocities) == self.n_dim + assert ( + len(dmp_point.positions) == self.n_dim + and len(dmp_point.velocities) == self.n_dim + ) dmp_trajectory.points.append(dmp_point) - dmp_trajectory.times = demo['times'] + dmp_trajectory.times = demo["times"] req.demo = dmp_trajectory req.k_gains = [self.k] * self.n_dim @@ -59,12 +62,21 @@ def set_active_dmp(self): req.dmp_list = self.dmp_list resp = self._set_client.call(req) if resp.success: - rospy.loginfo('DMP set') + rospy.loginfo("DMP set") else: - rospy.logerr('DMP not set!') + rospy.logerr("DMP not set!") - def get_dmp_plan(self, x_0, x_dot_0, goal, goal_thresh=0.001, - dt=1.0, t_0=0, seg_length=-1, integrate_iter=5): + def get_dmp_plan( + self, + x_0, + x_dot_0, + goal, + goal_thresh=0.001, + dt=1.0, + t_0=0, + seg_length=-1, + integrate_iter=5, + ): if self.tau is None: raise ValueError req = GetDMPPlanRequest() diff --git a/src/rotools/policy/rmp_flow/_example.py b/src/rotools/policy/rmp_flow/_example.py index 17cf6e1..06eff16 100644 --- a/src/rotools/policy/rmp_flow/_example.py +++ b/src/rotools/policy/rmp_flow/_example.py @@ -15,9 +15,9 @@ x_o = np.array([-1, 0, 0]) # center of the sphere obstacle r_o = 1.3 # radius of the obstacle -r = RMPRoot('root') -leaf1 = CollisionAvoidance('collision_avoidance', r, None, c=x_o, R=r_o, epsilon=0.2) -leaf2 = GoalAttractorUni('goal_attractor', r, x_g) +r = RMPRoot("root") +leaf1 = CollisionAvoidance("collision_avoidance", r, None, c=x_o, R=r_o, epsilon=0.2) +leaf2 = GoalAttractorUni("goal_attractor", r, x_g) x = np.array([2.5, -2, -1]) # init position x_dot = np.array([0, 0, 0]) # init velocity @@ -43,7 +43,7 @@ def dynamics(t, state): # -------------------------------------------- fig = plt.figure() -ax = fig.add_subplot(111, projection='3d') +ax = fig.add_subplot(111, projection="3d") u = np.linspace(0, 2 * np.pi, 30) v = np.linspace(0, np.pi, 30) @@ -51,21 +51,23 @@ def dynamics(t, state): ob_y = r_o * np.outer(np.sin(u), np.sin(v)) + x_o[1] ob_z = r_o * np.outer(np.ones(np.size(u)), np.cos(v)) + x_o[2] -ax.plot_surface(ob_x, ob_y, ob_z, rstride=1, cstride=1, cmap='rainbow', edgecolor='none') +ax.plot_surface( + ob_x, ob_y, ob_z, rstride=1, cstride=1, cmap="rainbow", edgecolor="none" +) # draw trajectory -ax.plot3D(sol.y[0], sol.y[1], sol.y[2], 'gray') +ax.plot3D(sol.y[0], sol.y[1], sol.y[2], "gray") # draw starting and ending point se = np.stack((x, x_g), axis=-1) -ax.scatter3D(se[0], se[1], se[2], c='gr') +ax.scatter3D(se[0], se[1], se[2], c="gr") fig_vel = plt.figure() ax_vel = fig_vel.add_subplot(111) -ax_vel.plot(sol.t, sol.y[3], color='red', linewidth=3) -ax_vel.plot(sol.t, sol.y[4], color='green', linewidth=3) -ax_vel.plot(sol.t, sol.y[5], color='blue', linewidth=3) +ax_vel.plot(sol.t, sol.y[3], color="red", linewidth=3) +ax_vel.plot(sol.t, sol.y[4], color="green", linewidth=3) +ax_vel.plot(sol.t, sol.y[5], color="blue", linewidth=3) plt.show() # -------------------------------------------- diff --git a/src/rotools/policy/rmp_flow/_leaf.py b/src/rotools/policy/rmp_flow/_leaf.py index 0be29d7..896cee5 100644 --- a/src/rotools/policy/rmp_flow/_leaf.py +++ b/src/rotools/policy/rmp_flow/_leaf.py @@ -12,8 +12,9 @@ class CollisionAvoidance(RMPLeaf): Obstacle avoidance RMP leaf """ - def __init__(self, name, parent, parent_param, c, R=1, epsilon=0.2, - alpha=1e-5, eta=0): + def __init__( + self, name, parent, parent_param, c, R=1, epsilon=0.2, alpha=1e-5, eta=0 + ): self.R = R self.alpha = alpha @@ -33,32 +34,38 @@ def __init__(self, name, parent, parent_param, c, R=1, epsilon=0.2, psi = lambda y: np.array(norm(y - c) / R - 1).reshape(-1, 1) J = lambda y: 1.0 / norm(y - c) * (y - c).T / R - J_dot = lambda y, y_dot: np.dot( - y_dot.T, - (-1 / norm(y - c) ** 3 * np.dot((y - c), (y - c).T) - + 1 / norm(y - c) * np.eye(N))) / R + J_dot = ( + lambda y, y_dot: np.dot( + y_dot.T, + ( + -1 / norm(y - c) ** 3 * np.dot((y - c), (y - c).T) + + 1 / norm(y - c) * np.eye(N) + ), + ) + / R + ) def RMP_func(x, x_dot): if x < 0: w = 1e10 grad_w = 0 else: - w = 1.0 / x ** 4 - grad_w = -4.0 / x ** 5 + w = 1.0 / x**4 + grad_w = -4.0 / x**5 u = epsilon + np.minimum(0, x_dot) * x_dot g = w * u grad_u = 2 * np.minimum(0, x_dot) grad_Phi = alpha * w * grad_w - xi = 0.5 * x_dot ** 2 * u * grad_w + xi = 0.5 * x_dot**2 * u * grad_w M = g + 0.5 * x_dot * w * grad_u - M = np.minimum(np.maximum(M, - 1e5), 1e5) + M = np.minimum(np.maximum(M, -1e5), 1e5) Bx_dot = eta * g * x_dot - f = - grad_Phi - xi - Bx_dot - f = np.minimum(np.maximum(f, - 1e10), 1e10) + f = -grad_Phi - xi - Bx_dot + f = np.minimum(np.maximum(f, -1e10), 1e10) return (f, M) @@ -70,8 +77,17 @@ class CollisionAvoidanceDecentralized(RMPLeaf): Decentralized collision avoidance RMP leaf for the RMPForest """ - def __init__(self, name, parent, parent_param, c=np.zeros(2), R=1, epsilon=1e-8, - alpha=1e-5, eta=0): + def __init__( + self, + name, + parent, + parent_param, + c=np.zeros(2), + R=1, + epsilon=1e-8, + alpha=1e-5, + eta=0, + ): assert parent_param is not None @@ -90,8 +106,8 @@ def RMP_func(x, x_dot, x_dot_real): w = 1e10 grad_w = 0 else: - w = 1.0 / x ** 4 - grad_w = -4.0 / x ** 5 + w = 1.0 / x**4 + grad_w = -4.0 / x**5 u = epsilon + np.minimum(0, x_dot) * x_dot g = w * u @@ -100,12 +116,12 @@ def RMP_func(x, x_dot, x_dot_real): xi = 0.5 * x_dot * x_dot_real * u * grad_w M = g + 0.5 * x_dot * w * grad_u - M = np.minimum(np.maximum(M, - 1e5), 1e5) + M = np.minimum(np.maximum(M, -1e5), 1e5) Bx_dot = eta * g * x_dot - f = - grad_Phi - xi - Bx_dot - f = np.minimum(np.maximum(f, - 1e10), 1e10) + f = -grad_Phi - xi - Bx_dot + f = np.minimum(np.maximum(f, -1e10), 1e10) return f, M @@ -116,14 +132,14 @@ def pushforward(self): override pushforward() to update the curvature term """ if self.verbose: - print('%s: pushforward' % self.name) + print("%s: pushforward" % self.name) if self.psi is not None and self.J is not None: self.x = self.psi(self.parent.x) self.x_dot = np.dot(self.J(self.parent.x), self.parent.x_dot) self.x_dot_real = np.dot( - self.J(self.parent.x), - self.parent.x_dot - self.parent_param.x_dot) + self.J(self.parent.x), self.parent.x_dot - self.parent_param.x_dot + ) def eval_leaf(self): """ @@ -145,10 +161,16 @@ def update_params(self): self.psi = lambda y: np.array(norm(y - c) / R - 1).reshape(-1, 1) self.J = lambda y: 1.0 / norm(y - c) * (y - c).T / R - self.J_dot = lambda y, y_dot: np.dot( - y_dot.T, - (-1 / norm(y - c) ** 3 * np.dot((y - c), (y - c).T) - + 1 / norm(y - c) * np.eye(N))) / R + self.J_dot = ( + lambda y, y_dot: np.dot( + y_dot.T, + ( + -1 / norm(y - c) ** 3 * np.dot((y - c), (y - c).T) + + 1 / norm(y - c) * np.eye(N) + ), + ) + / R + ) class CollisionAvoidanceCentralized(RMPLeaf): @@ -156,64 +178,77 @@ class CollisionAvoidanceCentralized(RMPLeaf): Centralized collision avoidance RMP leaf for a pair of robots """ - def __init__(self, name, parent, R=1, epsilon=1e-8, - alpha=1e-5, eta=0): + def __init__(self, name, parent, R=1, epsilon=1e-8, alpha=1e-5, eta=0): self.R = R def psi(y): N = int(y.size / 2) - y1 = y[: N] + y1 = y[:N] y2 = y[N:] return np.array(norm(y1 - y2) / R - 1).reshape(-1, 1) def J(y): N = int(y.size / 2) - y1 = y[: N] + y1 = y[:N] y2 = y[N:] - return np.concatenate(( - 1.0 / norm(y1 - y2) * (y1 - y2).T / R, - -1.0 / norm(y1 - y2) * (y1 - y2).T / R), - axis=1) + return np.concatenate( + ( + 1.0 / norm(y1 - y2) * (y1 - y2).T / R, + -1.0 / norm(y1 - y2) * (y1 - y2).T / R, + ), + axis=1, + ) def J_dot(y, y_dot): N = int(y.size / 2) - y1 = y[: N] + y1 = y[:N] y2 = y[N:] - y1_dot = y_dot[: N] + y1_dot = y_dot[:N] y2_dot = y_dot[N:] - return np.concatenate(( - np.dot( - y1_dot.T, - (-1 / norm(y1 - y2) ** 3 * np.dot((y1 - y2), (y1 - y2).T) - + 1 / norm(y1 - y2) * np.eye(N))) / R, - np.dot( - y2_dot.T, - (-1 / norm(y1 - y2) ** 3 * np.dot((y1 - y2), (y1 - y2).T) - + 1 / norm(y1 - y2) * np.eye(N))) / R), - axis=1) + return np.concatenate( + ( + np.dot( + y1_dot.T, + ( + -1 / norm(y1 - y2) ** 3 * np.dot((y1 - y2), (y1 - y2).T) + + 1 / norm(y1 - y2) * np.eye(N) + ), + ) + / R, + np.dot( + y2_dot.T, + ( + -1 / norm(y1 - y2) ** 3 * np.dot((y1 - y2), (y1 - y2).T) + + 1 / norm(y1 - y2) * np.eye(N) + ), + ) + / R, + ), + axis=1, + ) def RMP_func(x, x_dot): if x < 0: w = 1e10 grad_w = 0 else: - w = 1.0 / x ** 4 - grad_w = -4.0 / x ** 5 + w = 1.0 / x**4 + grad_w = -4.0 / x**5 u = epsilon + np.minimum(0, x_dot) * x_dot g = w * u grad_u = 2 * np.minimum(0, x_dot) grad_Phi = alpha * w * grad_w - xi = 0.5 * x_dot ** 2 * u * grad_w + xi = 0.5 * x_dot**2 * u * grad_w M = g + 0.5 * x_dot * w * grad_u - M = np.minimum(np.maximum(M, - 1e5), 1e5) + M = np.minimum(np.maximum(M, -1e5), 1e5) Bx_dot = eta * g * x_dot - f = - grad_Phi - xi - Bx_dot - f = np.minimum(np.maximum(f, - 1e10), 1e10) + f = -grad_Phi - xi - Bx_dot + f = np.minimum(np.maximum(f, -1e10), 1e10) return f, M @@ -225,8 +260,19 @@ class GoalAttractorUni(RMPLeaf): Goal Attractor RMP leaf """ - def __init__(self, name, parent, y_g, w_u=10, w_l=1, sigma=1, - alpha=1, eta=2, gain=3, tol=0.005): + def __init__( + self, + name, + parent, + y_g, + w_u=10, + w_l=1, + sigma=1, + alpha=1, + eta=2, + gain=3, + tol=0.005, + ): """ :param name: @@ -250,10 +296,9 @@ def __init__(self, name, parent, y_g, w_u=10, w_l=1, sigma=1, def RMP_func(x, x_dot): x_norm = norm(x) # 2-norm ||x|| - beta = np.exp(- x_norm ** 2 / 2 / (sigma ** 2)) + beta = np.exp(-(x_norm**2) / 2 / (sigma**2)) w = (w_u - w_l) * beta + w_l - s = (1 - np.exp(-2 * alpha * x_norm)) / (1 + np.exp( - -2 * alpha * x_norm)) + s = (1 - np.exp(-2 * alpha * x_norm)) / (1 + np.exp(-2 * alpha * x_norm)) G = w * np.eye(N) if x_norm > tol: @@ -261,14 +306,15 @@ def RMP_func(x, x_dot): else: grad_Phi = 0 Bx_dot = eta * w * x_dot # equivalent to M and G, eta the damping factor - grad_w = - beta * (w_u - w_l) / sigma ** 2 * x + grad_w = -beta * (w_u - w_l) / sigma**2 * x x_dot_norm = norm(x_dot) - xi = -0.5 * (x_dot_norm ** 2 * grad_w - 2 * - np.dot(np.dot(x_dot, x_dot.T), grad_w)) + xi = -0.5 * ( + x_dot_norm**2 * grad_w - 2 * np.dot(np.dot(x_dot, x_dot.T), grad_w) + ) M = G - f = - grad_Phi - Bx_dot - xi + f = -grad_Phi - Bx_dot - xi return f, M @@ -290,7 +336,9 @@ class FormationDecentralized(RMPLeaf): Decentralized formation control RMP leaf for the RMPForest """ - def __init__(self, name, parent, parent_param, c=np.zeros(2), d=1, gain=1, eta=2, w=1): + def __init__( + self, name, parent, parent_param, c=np.zeros(2), d=1, gain=1, eta=2, w=1 + ): assert parent_param is not None self.d = d @@ -303,7 +351,7 @@ def RMP_func(x, x_dot): grad_Phi = gain * x * w Bx_dot = eta * w * x_dot M = G - f = - grad_Phi - Bx_dot + f = -grad_Phi - Bx_dot return f, M @@ -327,8 +375,11 @@ def update_params(self): self.J = lambda y: 1.0 / norm(y - c) * (y - c).T self.J_dot = lambda y, y_dot: np.dot( y_dot.T, - (-1 / norm(y - c) ** 3 * np.dot((y - c), (y - c).T) - + 1 / norm(y - c) * np.eye(N))) + ( + -1 / norm(y - c) ** 3 * np.dot((y - c), (y - c).T) + + 1 / norm(y - c) * np.eye(N) + ), + ) class FormationCentralized(RMPLeaf): @@ -339,42 +390,51 @@ class FormationCentralized(RMPLeaf): def __init__(self, name, parent, d=1, gain=1, eta=2, w=1): def psi(y): N = int(y.size / 2) - y1 = y[: N] + y1 = y[:N] y2 = y[N:] return np.array(norm(y1 - y2) - d).reshape(-1, 1) def J(y): N = int(y.size / 2) - y1 = y[: N] + y1 = y[:N] y2 = y[N:] - return np.concatenate(( - 1.0 / norm(y1 - y2) * (y1 - y2).T, - -1.0 / norm(y1 - y2) * (y1 - y2).T), - axis=1) + return np.concatenate( + (1.0 / norm(y1 - y2) * (y1 - y2).T, -1.0 / norm(y1 - y2) * (y1 - y2).T), + axis=1, + ) def J_dot(y, y_dot): N = int(y.size / 2) - y1 = y[: N] + y1 = y[:N] y2 = y[N:] - y1_dot = y_dot[: N] + y1_dot = y_dot[:N] y2_dot = y_dot[N:] - return np.concatenate(( - np.dot( - y1_dot.T, - (-1 / norm(y1 - y2) ** 3 * np.dot((y1 - y2), (y1 - y2).T) - + 1 / norm(y1 - y2) * np.eye(N))), - np.dot( - y2_dot.T, - (-1 / norm(y1 - y2) ** 3 * np.dot((y1 - y2), (y1 - y2).T) - + 1 / norm(y1 - y2) * np.eye(N)))), - axis=1) + return np.concatenate( + ( + np.dot( + y1_dot.T, + ( + -1 / norm(y1 - y2) ** 3 * np.dot((y1 - y2), (y1 - y2).T) + + 1 / norm(y1 - y2) * np.eye(N) + ), + ), + np.dot( + y2_dot.T, + ( + -1 / norm(y1 - y2) ** 3 * np.dot((y1 - y2), (y1 - y2).T) + + 1 / norm(y1 - y2) * np.eye(N) + ), + ), + ), + axis=1, + ) def RMP_func(x, x_dot): G = w grad_Phi = gain * x * w Bx_dot = eta * w * x_dot M = G - f = - grad_Phi - Bx_dot + f = -grad_Phi - Bx_dot return (f, M) @@ -395,7 +455,7 @@ def RMP_func(x, x_dot): G = w Bx_dot = eta * w * x_dot M = G - f = - Bx_dot + f = -Bx_dot return f, M diff --git a/src/rotools/policy/rmp_flow/_root.py b/src/rotools/policy/rmp_flow/_root.py index 0f2d2d1..c9503e6 100644 --- a/src/rotools/policy/rmp_flow/_root.py +++ b/src/rotools/policy/rmp_flow/_root.py @@ -50,7 +50,7 @@ def pushforward(self): """ if self.verbose: - print('%s: pushforward' % self.name) + print("%s: pushforward" % self.name) if self.psi is not None and self.J is not None: self.x = self.psi(self.parent.x) @@ -68,11 +68,10 @@ def pullback(self): [child.pullback() for child in self.children] if self.verbose: - print('%s: pullback' % self.name) + print("%s: pullback" % self.name) - f = np.zeros_like(self.x, dtype='float64') - M = np.zeros((max(self.x.shape), max(self.x.shape)), - dtype='float64') + f = np.zeros_like(self.x, dtype="float64") + M = np.zeros((max(self.x.shape), max(self.x.shape)), dtype="float64") for child in self.children: @@ -82,8 +81,10 @@ def pullback(self): assert J_child.ndim == 2 and J_dot_child.ndim == 2 if child.f is not None and child.M is not None: - f += np.dot(J_child.T, (child.f - np.dot( - np.dot(child.M, J_dot_child), self.x_dot))) + f += np.dot( + J_child.T, + (child.f - np.dot(np.dot(child.M, J_dot_child), self.x_dot)), + ) M += np.dot(np.dot(J_child.T, child.M), J_child) self.f = f @@ -116,7 +117,7 @@ def pushforward(self): """ if self.verbose: - print('%s: pushforward' % self.name) + print("%s: pushforward" % self.name) [child.pushforward() for child in self.children] @@ -124,7 +125,7 @@ def resolve(self): """compute the canonical-formed RMP.""" if self.verbose: - print('%s: resolve' % self.name) + print("%s: resolve" % self.name) self.a = np.dot(np.linalg.pinv(self.M), self.f) return self.a @@ -155,11 +156,10 @@ def eval_leaf(self): self.f, self.M = self.RMP_func(self.x, self.x_dot) def pullback(self): - """pullback at leaf node is just evaluating the RMP - """ + """pullback at leaf node is just evaluating the RMP""" if self.verbose: - print('%s: pullback' % self.name) + print("%s: pullback" % self.name) self.eval_leaf() diff --git a/src/rotools/policy/rmp_flow/planner.py b/src/rotools/policy/rmp_flow/planner.py index 1f4aa5e..d529194 100644 --- a/src/rotools/policy/rmp_flow/planner.py +++ b/src/rotools/policy/rmp_flow/planner.py @@ -47,15 +47,23 @@ def plan(self, state, goal, obstacle=None, span=None): # For now, the root and leaf need to be explicitly initialized, otherwise # some temporary state will make the planning go wrong TODO figure the reason - self._root = RMPRoot('root') - leaf_goal = GoalAttractorUni('goal_attractor', self._root, goal, gain=1) + self._root = RMPRoot("root") + leaf_goal = GoalAttractorUni("goal_attractor", self._root, goal, gain=1) if obstacle is not None: - leaf_obs = CollisionAvoidance('collision_avoidance', self._root, - None, c=obstacle[:3], R=obstacle[3], epsilon=0.2) + leaf_obs = CollisionAvoidance( + "collision_avoidance", + self._root, + None, + c=obstacle[:3], + R=obstacle[3], + epsilon=0.2, + ) sol = solve_ivp(self._dynamics, span, state) if sol: - positions = sol.y[:self.dim, :] # shape (dof, N), N is the way point number - velocities = sol.y[self.dim:, :] + positions = sol.y[ + : self.dim, : + ] # shape (dof, N), N is the way point number + velocities = sol.y[self.dim :, :] timestamps = sol.t return timestamps, positions, velocities # accelerations else: diff --git a/src/rotools/robot/serial/geometry.py b/src/rotools/robot/serial/geometry.py index d241cb0..fbe2db4 100755 --- a/src/rotools/robot/serial/geometry.py +++ b/src/rotools/robot/serial/geometry.py @@ -69,7 +69,7 @@ def vector_2_matrix(vector, convention=OrientationConvention.EULER_ZYX): def position_from_matrix(matrix): """Get the position values from a 4x4 transform matrix. - + :param matrix: np.ndarray :return np.ndarray """ diff --git a/src/rotools/robot/serial/kinematic_chain.py b/src/rotools/robot/serial/kinematic_chain.py index dd46b68..dfa87ca 100755 --- a/src/rotools/robot/serial/kinematic_chain.py +++ b/src/rotools/robot/serial/kinematic_chain.py @@ -210,7 +210,9 @@ def __attrs_post_init__(self): @classmethod def from_parameters(cls, poe): """Construct Kinematic Chain from parameters.""" - assert len(poe) == 2, print('POE params should contain home matrix and screw axes') + assert len(poe) == 2, print( + "POE params should contain home matrix and screw axes" + ) home_matrix = poe[0] screw_axes = poe[1] kc = cls(home_matrix=home_matrix, screw_axes=screw_axes) diff --git a/src/rotools/robot/serial/model.py b/src/rotools/robot/serial/model.py index 466f364..f5817ea 100755 --- a/src/rotools/robot/serial/model.py +++ b/src/rotools/robot/serial/model.py @@ -17,7 +17,7 @@ def config_factory(robot): if robot.mdh is not None or robot.poe is not None: return np.zeros(robot.dof, dtype=float) else: - raise ValueError('No MDH or POE param defined') + raise ValueError("No MDH or POE param defined") def config_limits_factory(robot): @@ -38,7 +38,9 @@ class RobotModel(Sized): mdh = attrib(default=None, type=MDHKinematicChain) # For transforming eef pose in the arm's base frame to the robot's base frame or getting the inverse transform. - arm_base_to_robot_base_trans = attrib(factory=lambda: transform.identity_matrix(), type=np.ndarray) + arm_base_to_robot_base_trans = attrib( + factory=lambda: transform.identity_matrix(), type=np.ndarray + ) random_state = attrib( factory=lambda: np.random.RandomState(), @@ -90,7 +92,9 @@ def fk(self, q): pose = robotics.fk_in_space(self.poe.home_matrix, self.poe.screw_axes, q) else: transforms = [self.world_frame] - transforms.extend(self.mdh.transforms(q)) # Add n=dof transforms to the list + transforms.extend( + self.mdh.transforms(q) + ) # Add n=dof transforms to the list transforms.append(self.tool.matrix) # matrix multiply through transforms @@ -103,7 +107,9 @@ def partial_fk(self, q, i): if i >= self.dof: raise IndexError transforms = [] - transforms.extend(self.mdh.transforms(q[:i + 1])) # Add n=dof transforms to the list + transforms.extend( + self.mdh.transforms(q[: i + 1]) + ) # Add n=dof transforms to the list # matrix multiply through transforms pose = np.eye(4, dtype=float) @@ -133,12 +139,22 @@ def ik_in_space(self, target_pose, current_q): """ target_pose = common.sd_pose(target_pose, check=True) if self.poe is not None: - target_q, ok = robotics.ik_in_space(self.poe.screw_axes, self.poe.home_matrix, target_pose, - current_q, 1e-2, 1e-3) + target_q, ok = robotics.ik_in_space( + self.poe.screw_axes, + self.poe.home_matrix, + target_pose, + current_q, + 1e-2, + 1e-3, + ) return target_q if ok else None - result = scipy.optimize.least_squares(fun=_ik_cost_function, x0=current_q, - bounds=self.q_limits, args=(self, target_pose)) + result = scipy.optimize.least_squares( + fun=_ik_cost_function, + x0=current_q, + bounds=self.q_limits, + args=(self, target_pose), + ) if result.success: # pragma: no cover actual_pose = self.fk(result.x) @@ -155,14 +171,14 @@ def mdh_to_poe(self): for i in range(self.dof): q = np.zeros(i + 1) trans_0 = self.partial_fk(q, i) - q[-1] = np.pi / 2. + q[-1] = np.pi / 2.0 trans_1 = self.partial_fk(q, i) trans_rot = np.dot(np.linalg.inv(trans_0), trans_1) # This is the rotation axis of joint i in its frame, where 0, 1, 2 for x, y, z local_axis_i = 0 for col in range(3): - if trans_rot[col, col] == 1.: + if trans_rot[col, col] == 1.0: local_axis_i = col # Get the global axis corresponding to the local axis, which is by definition the omega component of S. @@ -212,7 +228,9 @@ def dq(self, value): :return: """ if np.any(value < self.dq_limits[0]) or np.any(value > self.dq_limits[1]): - raise ValueError('Velocity to set {} out of permissible range'.format(value)) + raise ValueError( + "Velocity to set {} out of permissible range".format(value) + ) self._dq = value @property @@ -222,7 +240,9 @@ def dq_limits(self): @dq_limits.setter def dq_limits(self, value): if value.shape[0] != 2 or value.shape[1] != len(self): - raise ValueError('Velocity limits should have the shape (2, dof), 2 for min max') + raise ValueError( + "Velocity limits should have the shape (2, dof), 2 for min max" + ) self._dq_limits = value @property @@ -244,8 +264,10 @@ def q_limits(self, value): self._q_limits = value.T else: raise ValueError( - 'Joint limits should have the shape (2, dof), 2 for min max, but the given shape is {}'.format( - value.shape)) + "Joint limits should have the shape (2, dof), 2 for min max, but the given shape is {}".format( + value.shape + ) + ) def jacobian_space(self, q=None): """Calculate the Jacobian wrt the world frame.""" @@ -278,11 +300,16 @@ def jacobian_flange(self, q=None): current_trans = self.tool.matrix.copy() for i in reversed(range(self.dof)): - d = np.array([ - -current_trans[0, 0] * current_trans[1, 3] + current_trans[1, 0] * current_trans[0, 3], - -current_trans[0, 1] * current_trans[1, 3] + current_trans[1, 1] * current_trans[0, 3], - -current_trans[0, 2] * current_trans[1, 3] + current_trans[1, 2] * current_trans[0, 3], - ]) + d = np.array( + [ + -current_trans[0, 0] * current_trans[1, 3] + + current_trans[1, 0] * current_trans[0, 3], + -current_trans[0, 1] * current_trans[1, 3] + + current_trans[1, 1] * current_trans[0, 3], + -current_trans[0, 2] * current_trans[1, 3] + + current_trans[1, 2] * current_trans[0, 3], + ] + ) delta = current_trans[2, 0:3] jacobian_flange[:, i] = np.hstack((d, delta)) current_link = self.mdh.links[i] diff --git a/src/rotools/robot/serial/optimization.py b/src/rotools/robot/serial/optimization.py index cf44128..707f860 100755 --- a/src/rotools/robot/serial/optimization.py +++ b/src/rotools/robot/serial/optimization.py @@ -82,9 +82,7 @@ def apply_optimization_vector(self, vector): def generate_optimization_vector(self): """Generate vector.""" - kc_vector = np.compress( - self.kinematic_chain_mask, self.robot.mdh.vector - ) + kc_vector = np.compress(self.kinematic_chain_mask, self.robot.mdh.vector) tool_vector = np.compress(self.tool_mask, self.robot.tool.vector) world_vector = np.compress( self.world_mask, matrix_2_vector(self.robot.world_frame) @@ -101,7 +99,9 @@ def optimize_accuracy(optimization_vector, handler, qs, positions): :param positions: Sequence[Sequence[float]] """ handler.apply_optimization_vector(optimization_vector) - errors = compute_absolute_errors(qs=qs, positions=positions, robot=handler.commander) + errors = compute_absolute_errors( + qs=qs, positions=positions, robot=handler.commander + ) return errors diff --git a/src/rotools/robot/serial/predefined_models.py b/src/rotools/robot/serial/predefined_models.py index 1c3a384..31b76e3 100755 --- a/src/rotools/robot/serial/predefined_models.py +++ b/src/rotools/robot/serial/predefined_models.py @@ -11,18 +11,27 @@ def ur10e_mdh_model(): ndarray, ndarray MDH params and joint limits. """ - mdh = np.array([ - [0, 0, 0, 0.1807], - [np.pi / 2, 0, np.pi, 0], - [0, 0.6127, 0, 0], - [0, 0.57155, 0, 0.17415], - [-np.pi / 2, 0, 0, 0.11985], - [np.pi / 2, 0, np.pi, 0.11655], - ]) - - q_limits = np.array([ - [-np.pi, np.pi], [-np.pi, np.pi], [-np.pi, np.pi], [-np.pi, np.pi], [-np.pi, np.pi], [-np.pi, np.pi], - ]) + mdh = np.array( + [ + [0, 0, 0, 0.1807], + [np.pi / 2, 0, np.pi, 0], + [0, 0.6127, 0, 0], + [0, 0.57155, 0, 0.17415], + [-np.pi / 2, 0, 0, 0.11985], + [np.pi / 2, 0, np.pi, 0.11655], + ] + ) + + q_limits = np.array( + [ + [-np.pi, np.pi], + [-np.pi, np.pi], + [-np.pi, np.pi], + [-np.pi, np.pi], + [-np.pi, np.pi], + [-np.pi, np.pi], + ] + ) return mdh, q_limits @@ -62,25 +71,36 @@ def ur10e_poe_model(): list[ndarray], ndarray. POE params and joint limits. """ - M = np.array([ - [1, 0, 0, -1.18425], # L1+L2 - [0, 0, -1, -0.2907], # W1+W2 - [0, 1, 0, 0.06085], # H1-H2 - [0, 0, 0, 1], - ]) - - screw_axes = np.array([ - [0, 0, 1, 0, 0, 0], - [0, -1, 0, 0.1807, 0, 0], # -H1 - [0, -1, 0, 0.1807, 0, 0.6127], # -H1 L1 - [0, -1, 0, 0.1807, 0, 1.18425], # -H1 L1+L2 - [0, 0, -1, 0.17415, -1.18425, 0], # -W1 L1+L2 - [0, -1, 0, 0.06085, 0, 1.18425], # H2-H1 L1+L2 - ]).T - - q_limits = np.array([ - [-np.pi, np.pi], [-np.pi, np.pi], [-np.pi, np.pi], [-np.pi, np.pi], [-np.pi, np.pi], [-np.pi, np.pi], - ]) + M = np.array( + [ + [1, 0, 0, -1.18425], # L1+L2 + [0, 0, -1, -0.2907], # W1+W2 + [0, 1, 0, 0.06085], # H1-H2 + [0, 0, 0, 1], + ] + ) + + screw_axes = np.array( + [ + [0, 0, 1, 0, 0, 0], + [0, -1, 0, 0.1807, 0, 0], # -H1 + [0, -1, 0, 0.1807, 0, 0.6127], # -H1 L1 + [0, -1, 0, 0.1807, 0, 1.18425], # -H1 L1+L2 + [0, 0, -1, 0.17415, -1.18425, 0], # -W1 L1+L2 + [0, -1, 0, 0.06085, 0, 1.18425], # H2-H1 L1+L2 + ] + ).T + + q_limits = np.array( + [ + [-np.pi, np.pi], + [-np.pi, np.pi], + [-np.pi, np.pi], + [-np.pi, np.pi], + [-np.pi, np.pi], + [-np.pi, np.pi], + ] + ) return [M, screw_axes], q_limits @@ -105,18 +125,35 @@ def panda_mdh_model(): [0, 0, 0, 0.333], # theta in [-2.8973 2.8973] 0 -> 1 [-np.pi / 2, 0, 0, 0], # theta in [-1.7628 1.7628] 1 -> 2 [np.pi / 2, 0, 0, 0.316], # theta in [-2.8973 2.8973] 2 -> 3 - [np.pi / 2, 0.088, 0, 0], # theta in [-3.0718 -0.0698], note the ref value of theta is 0, 3 -> 4 + [ + np.pi / 2, + 0.088, + 0, + 0, + ], # theta in [-3.0718 -0.0698], note the ref value of theta is 0, 3 -> 4 [-np.pi / 2, -0.088, 0, 0.384], # theta in [-2.8973 2.8973] 4 -> 5 [np.pi / 2, 0, 0, 0], # theta in [-0.0175 3.7525] 5 -> 6 - [np.pi / 2, 0.088, 0, 0.107], # theta in [-2.8973 2.8973], 0.107 is distance between link 6 and 8 + [ + np.pi / 2, + 0.088, + 0, + 0.107, + ], # theta in [-2.8973 2.8973], 0.107 is distance between link 6 and 8 ], - dtype=float + dtype=float, ) - q_limits = np.array([ - [-2.8973, 2.8973], [-1.7628, 1.7628], [-2.8973, 2.8973], [-3.0718, -0.0698], [-2.8973, 2.8973], - [-0.0175, 3.7525], [-2.8973, 2.8973] - ]) + q_limits = np.array( + [ + [-2.8973, 2.8973], + [-1.7628, 1.7628], + [-2.8973, 2.8973], + [-3.0718, -0.0698], + [-2.8973, 2.8973], + [-0.0175, 3.7525], + [-2.8973, 2.8973], + ] + ) return mdh, q_limits @@ -128,27 +165,38 @@ def panda_poe_model(): list[ndarray], ndarray POE params and joint limits. """ - M = np.array([ - [1, 0, 0, 0.088], - [0, -1, 0, 0], - [0, 0, -1, 0.926], - [0, 0, 0, 1], - ]) - - screw_axes = np.array([ - [0, 0, 1, 0, 0, 0], - [0, 1, 0, -0.333, 0, 0], - [0, 0, 1, 0, 0, 0], - [0, -1, 0, 0.649, 0, -0.088], - [0, 0, 1, 0, 0, 0], - [0, -1, 0, 1.033, 0, 0], - [0, 0, -1, 0, 0.088, 0], - ]).T - - q_limits = np.array([ - [-2.8973, 2.8973], [-1.7628, 1.7628], [-2.8973, 2.8973], [-3.0718, -0.0698], [-2.8973, 2.8973], - [-0.0175, 3.7525], [-2.8973, 2.8973] - ]) + M = np.array( + [ + [1, 0, 0, 0.088], + [0, -1, 0, 0], + [0, 0, -1, 0.926], + [0, 0, 0, 1], + ] + ) + + screw_axes = np.array( + [ + [0, 0, 1, 0, 0, 0], + [0, 1, 0, -0.333, 0, 0], + [0, 0, 1, 0, 0, 0], + [0, -1, 0, 0.649, 0, -0.088], + [0, 0, 1, 0, 0, 0], + [0, -1, 0, 1.033, 0, 0], + [0, 0, -1, 0, 0.088, 0], + ] + ).T + + q_limits = np.array( + [ + [-2.8973, 2.8973], + [-1.7628, 1.7628], + [-2.8973, 2.8973], + [-3.0718, -0.0698], + [-2.8973, 2.8973], + [-0.0175, 3.7525], + [-2.8973, 2.8973], + ] + ) return [M, screw_axes], q_limits @@ -160,29 +208,35 @@ def walker_left_arm_poe(): verified date: 2020/6/11 """ # home matrix (homogeneous matrix transform base frame to eef frame) - M = np.array([ - [9.98925860e-01, 4.63370949e-02, -1.70247991e-07, 4.51820844e-01], - [3.40368002e-07, -1.10117044e-05, -1.00000000e+00, -8.40714493e-08], - [-4.63370949e-02, 9.98925860e-01, -1.10156479e-05, -1.02911897e-02], - [0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00] - ]) + M = np.array( + [ + [9.98925860e-01, 4.63370949e-02, -1.70247991e-07, 4.51820844e-01], + [3.40368002e-07, -1.10117044e-05, -1.00000000e00, -8.40714493e-08], + [-4.63370949e-02, 9.98925860e-01, -1.10156479e-05, -1.02911897e-02], + [0.00000000e00, 0.00000000e00, 0.00000000e00, 1.00000000e00], + ] + ) # screw axes all relative to the base_frame (fixed relative to the robot base) # to calculate rotation axis, we first align the origin of the frame in query # to that of the base frame - screw_axes = np.array([ - [0, 0, -1, 0, 0, 0], - [0, 1, 0, 0, 0, 0], - [1, 0, 0, 0, 0, 0], - [0, -1, 0, -2.47791593e-02, 0, -2.28499909e-01], - [1, 0, 0, 0, 0, 0], - [0, 0, -1, 0, 4.51820844e-01, 0], - [0, 1, 0, 0, 0, 4.51820844e-01] - ]).T + screw_axes = np.array( + [ + [0, 0, -1, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [1, 0, 0, 0, 0, 0], + [0, -1, 0, -2.47791593e-02, 0, -2.28499909e-01], + [1, 0, 0, 0, 0, 0], + [0, 0, -1, 0, 4.51820844e-01, 0], + [0, 1, 0, 0, 0, 4.51820844e-01], + ] + ).T # transform from robot base link to arm base link (i.e., base_to_s) - base_to_static = np.array([ - [-9.56714879e-07, -1., -3.73952529e-06, 0], - [2.60457789e-01, -3.85964030e-06, 9.65485236e-01, 2.41420000e-01], - [-9.65485236e-01, -5.02943990e-08, 2.60457789e-01, 1.83860000e-02], - [0., 0., 0., 1.] - ]) + base_to_static = np.array( + [ + [-9.56714879e-07, -1.0, -3.73952529e-06, 0], + [2.60457789e-01, -3.85964030e-06, 9.65485236e-01, 2.41420000e-01], + [-9.65485236e-01, -5.02943990e-08, 2.60457789e-01, 1.83860000e-02], + [0.0, 0.0, 0.0, 1.0], + ] + ) return M, screw_axes, base_to_static diff --git a/src/rotools/robot/serial/tool.py b/src/rotools/robot/serial/tool.py index d530b6f..a858535 100755 --- a/src/rotools/robot/serial/tool.py +++ b/src/rotools/robot/serial/tool.py @@ -3,7 +3,11 @@ from typing import Sized from attr import attrs, attrib, Factory -from rotools.robot.serial.geometry import matrix_2_vector, position_from_matrix, vector_2_matrix +from rotools.robot.serial.geometry import ( + matrix_2_vector, + position_from_matrix, + vector_2_matrix, +) @attrs diff --git a/src/rotools/sensing/core/interface.py b/src/rotools/sensing/core/interface.py index ed0b8cd..6241529 100644 --- a/src/rotools/sensing/core/interface.py +++ b/src/rotools/sensing/core/interface.py @@ -20,30 +20,35 @@ class SensingInterface(object): - def __init__( - self, - device_names, - algorithm_ports, - algorithm_names=None, + self, + device_names, + algorithm_ports, + algorithm_names=None, ): super(SensingInterface, self).__init__() self._bridge = CvBridge() - self._pose_publisher = rospy.Publisher('roport_visualization/pose', GeometryMsg.PoseStamped, queue_size=1) + self._pose_publisher = rospy.Publisher( + "roport_visualization/pose", GeometryMsg.PoseStamped, queue_size=1 + ) if not isinstance(device_names, list) and not isinstance(device_names, tuple): - raise TypeError('device_names should be list or tuple, but got {}'.format(type(device_names))) + raise TypeError( + "device_names should be list or tuple, but got {}".format( + type(device_names) + ) + ) - assert len(device_names) > 0, rospy.logerr('No sensing device given') + assert len(device_names) > 0, rospy.logerr("No sensing device given") self.device_names = device_names self.device_ids = np.arange(len(self.device_names)) for i, name in enumerate(self.device_names): - rospy.loginfo('Assigning id {} to the device {}'.format(i, name)) + rospy.loginfo("Assigning id {} to the device {}".format(i, name)) # TODO check device status - assert len(algorithm_ports) > 0, rospy.logerr('No sensing algorithm given') + assert len(algorithm_ports) > 0, rospy.logerr("No sensing algorithm given") self.algorithm_ports = algorithm_ports self.algorithm_ids = np.arange(len(self.algorithm_ports)) @@ -51,7 +56,7 @@ def __init__( assert len(algorithm_names) == len(self.algorithm_ports) self.algorithm_names = algorithm_names for name, port in zip(self.algorithm_names, self.algorithm_ports): - rospy.loginfo('Algorithm \'{}\' available at {}'.format(name, port)) + rospy.loginfo("Algorithm '{}' available at {}".format(name, port)) else: self.algorithm_names = None @@ -61,22 +66,24 @@ def sense_manipulation_poses(self, device_names, algorithm_id, data_types=None): """Sensing the manipulation poses (i.e., the poses that the robot's end-effector should move to to perform manipulation.) By default, these poses are wrt the sensor's frame. - + :param device_names: str Names of the device. - :param algorithm_id: int ID of the algorithm. + :param algorithm_id: int ID of the algorithm. :param data_types: int Type ID of the data. If not given, the method will guess it based on the device name. """ for name in device_names: - assert name in self.device_names, rospy.logerr('Device name {} is not registered'.format(name)) + assert name in self.device_names, rospy.logerr( + "Device name {} is not registered".format(name) + ) assert algorithm_id in self.algorithm_ids if data_types is None: temp_types = [] for name in device_names: - if 'rgb' in name or 'RGB' in name or 'color' in name: + if "rgb" in name or "RGB" in name or "color" in name: temp_types.append(0) - elif 'depth' in name: + elif "depth" in name: temp_types.append(1) else: raise NotImplementedError @@ -88,7 +95,9 @@ def sense_manipulation_poses(self, device_names, algorithm_id, data_types=None): if not ok: return False, None, None # Send sensory data to the algorithm and get the poses - ok, sd_poses = self._post_data(self.algorithm_ports[algorithm_id], data, data_types) + ok, sd_poses = self._post_data( + self.algorithm_ports[algorithm_id], data, data_types + ) if ok: return True, common.to_ros_poses(sd_poses), common.to_ros_pose(sd_poses[0]) else: @@ -104,7 +113,7 @@ def _get_image_data_client(self, device_names, service_name=None): :return: ok, list[ndarray] """ if service_name is None: - service_name = 'get_image_data' + service_name = "get_image_data" rospy.wait_for_service(service_name, 1000) # wait for 1s try: get_img = rospy.ServiceProxy(service_name, GetImageData) @@ -114,7 +123,9 @@ def _get_image_data_client(self, device_names, service_name=None): if resp.result_status == resp.SUCCEEDED: image_list = [] for img_msg in resp.images: - cv_img = self._bridge.imgmsg_to_cv2(img_msg, desired_encoding='passthrough') + cv_img = self._bridge.imgmsg_to_cv2( + img_msg, desired_encoding="passthrough" + ) image_list.append(cv_img) return True, image_list else: @@ -156,16 +167,16 @@ def _post_data(self, port, data, data_types): type_list.append(dt) data_list.append(str(encoded)) - body = {'data': json.dumps(data_list), 'data_types': json.dumps(data_types)} - payload = {'header': header, 'body': body} - feedback = common.post_http_requests('http://{}'.format(port), payload=payload) + body = {"data": json.dumps(data_list), "data_types": json.dumps(data_types)} + payload = {"header": header, "body": body} + feedback = common.post_http_requests("http://{}".format(port), payload=payload) results = feedback.json() # The keys 'status' and 'results' should be coincide with the definition in the Flask server - ok = results['response']['status'] + ok = results["response"]["status"] if ok: - return True, np.array(json.loads(results['response']['results'])) + return True, np.array(json.loads(results["response"]["results"])) else: - rospy.logerr('Post data failed to load results') + rospy.logerr("Post data failed to load results") return False, None def visualize_pose(self, pose, frame): diff --git a/src/rotools/sensing/core/server.py b/src/rotools/sensing/core/server.py index ff6e533..0fbc5a5 100644 --- a/src/rotools/sensing/core/server.py +++ b/src/rotools/sensing/core/server.py @@ -9,7 +9,6 @@ class SensingServer(object): - def __init__(self, kwargs): super(SensingServer, self).__init__() @@ -17,17 +16,23 @@ def __init__(self, kwargs): self.interface = interface.SensingInterface(**kwargs) # Sensing services - self._srv_sense_pose = rospy.Service('sense_manipulation_poses', SenseManipulationPoses, self.sense_pose_handle) + self._srv_sense_pose = rospy.Service( + "sense_manipulation_poses", SenseManipulationPoses, self.sense_pose_handle + ) # Information getters # General utilities - self._srv_visualize_pose = rospy.Service('visualize_pose', VisualizePose, self.visualize_pose_handle) + self._srv_visualize_pose = rospy.Service( + "visualize_pose", VisualizePose, self.visualize_pose_handle + ) def sense_pose_handle(self, req): resp = SenseManipulationPosesResponse() try: - ok, poses, best_pose = self.interface.sense_manipulation_poses(req.device_names, req.algorithm_id) + ok, poses, best_pose = self.interface.sense_manipulation_poses( + req.device_names, req.algorithm_id + ) if ok: resp.result_status = resp.SUCCEEDED resp.poses = poses diff --git a/src/rotools/simulation/mujoco/interface.py b/src/rotools/simulation/mujoco/interface.py index 1eb944a..e453813 100644 --- a/src/rotools/simulation/mujoco/interface.py +++ b/src/rotools/simulation/mujoco/interface.py @@ -1,6 +1,5 @@ from __future__ import print_function -import copy import os import time import mujoco @@ -11,9 +10,20 @@ from threading import Thread from rotools.simulation.mujoco.mujoco_viewer import MujocoViewer -from rotools.utility.mjcf import find_elements, find_parent, array_to_string, string_to_array -from rotools.utility.common import to_ros_pose, to_ros_twist, to_list, all_close, get_transform_same_origin, \ - to_ros_orientation +from rotools.utility.mjcf import ( + find_elements, + find_parent, + array_to_string, + string_to_array, +) +from rotools.utility.common import ( + to_ros_pose, + to_ros_twist, + to_list, + all_close, + get_transform_same_origin, + to_ros_orientation, +) from rotools.utility.color_palette import bwr_color_palette try: @@ -36,13 +46,13 @@ class MuJoCoInterface(Thread): """ def __init__( - self, - model_path, - kinematics_path=None, - actuator_path=None, - enable_viewer=True, - verbose=False, - **kwargs + self, + model_path, + kinematics_path=None, + actuator_path=None, + enable_viewer=True, + verbose=False, + **kwargs ): """Initialize the MuJoCoInterface. @@ -60,7 +70,9 @@ def __init__( self._enable_viewer = enable_viewer if not os.path.exists(model_path): - raise FileNotFoundError("Model XML file '{}' does not exist".format(model_path)) + raise FileNotFoundError( + "Model XML file '{}' does not exist".format(model_path) + ) self._model_path = model_path # MuJoCo objects @@ -84,7 +96,7 @@ def __init__( kinematic_tree = ElementTree.parse(kinematics_path) kinematics_root = kinematic_tree.getroot() # This robot name will also refer to the base frame of the robot - self.robot_name = find_elements(kinematics_root, 'body').attrib['name'] + self.robot_name = find_elements(kinematics_root, "body").attrib["name"] if self.robot_name is not None: rospy.loginfo("Robot name: {}".format(self.robot_name)) else: @@ -92,20 +104,26 @@ def __init__( else: self.robot_name = None kinematics_root = None - rospy.logwarn("Kinematics XML file '{}' does not exist".format(kinematics_path)) + rospy.logwarn( + "Kinematics XML file '{}' does not exist".format(kinematics_path) + ) if os.path.exists(actuator_path): actuator_tree = ElementTree.parse(actuator_path) actuator_root = actuator_tree.getroot() - position_actuators = find_elements(actuator_root, 'position', return_first=False) - velocity_actuators = find_elements(actuator_root, 'velocity', return_first=False) - torque_actuators = find_elements(actuator_root, 'motor', return_first=False) + position_actuators = find_elements( + actuator_root, "position", return_first=False + ) + velocity_actuators = find_elements( + actuator_root, "velocity", return_first=False + ) + torque_actuators = find_elements(actuator_root, "motor", return_first=False) - mimic_joints = find_elements(actuator_root, 'joint', return_first=False) + mimic_joints = find_elements(actuator_root, "joint", return_first=False) - torque_sensors = find_elements(actuator_root, 'torque', return_first=False) - force_sensors = find_elements(actuator_root, 'force', return_first=False) + torque_sensors = find_elements(actuator_root, "torque", return_first=False) + force_sensors = find_elements(actuator_root, "force", return_first=False) """ control_types: list[int] How the actuator is controlled. @@ -121,28 +139,34 @@ def __init__( self._get_actuated_joint_geoms(kinematics_root) self._actuator_num = len(self.actuator_names) - rospy.loginfo('Controlled joints #{}:\n{}'.format( - self._actuator_num, array_to_string(self._actuated_joint_names)) + rospy.loginfo( + "Controlled joints #{}:\n{}".format( + self._actuator_num, array_to_string(self._actuated_joint_names) + ) ) rospy.loginfo( - 'Control types: position (0), velocity (1), torque (2).\n{}'.format( - array_to_string(self.control_types))) + "Control types: position (0), velocity (1), torque (2).\n{}".format( + array_to_string(self.control_types) + ) + ) - self._null_sensor = 'none' + self._null_sensor = "none" self.effort_sensor_names = [self._null_sensor] * self._actuator_num self._get_effort_sensor_info(kinematics_root, torque_sensors) self._get_effort_sensor_info(kinematics_root, force_sensors) - rospy.loginfo('Effort sensors:\n{}'.format(array_to_string(self.effort_sensor_names))) + rospy.loginfo( + "Effort sensors:\n{}".format(array_to_string(self.effort_sensor_names)) + ) else: rospy.logwarn("Actuator XML file '{}' does not exist".format(actuator_path)) self._robot_states = None - self._clock_publisher = rospy.Publisher('/clock', Clock, queue_size=1) + self._clock_publisher = rospy.Publisher("/clock", Clock, queue_size=1) - self._tracked_object_name = 'object' + self._tracked_object_name = "object" self._neutral_initialized = False @@ -154,7 +178,11 @@ def set_tracked_object(self, name): self._data.body(name) self._tracked_object_name = name except KeyError: - rospy.logwarn('Failed to set object to track since the model does not have body {}'.format(name)) + rospy.logwarn( + "Failed to set object to track since the model does not have body {}".format( + name + ) + ) @property def n_actuator(self): @@ -169,7 +197,9 @@ def run(self): except KeyError: pass self._data = mujoco.MjData(self._model) - self._viewer = MujocoViewer(self._model, self._data) if self._enable_viewer else None + self._viewer = ( + MujocoViewer(self._model, self._data) if self._enable_viewer else None + ) start = time.time() clock_msg = Clock() @@ -199,12 +229,12 @@ def _neutral_initialization(self): ctrl_range = self._actuator_ctrl_ranges[actuator_name] if ctrl_range is not None: low, high = ctrl_range - if 0. <= low: - neutral = low + (high - low) / 10. - elif high <= 0.: - neutral = high - (high - low) / 10. + if 0.0 <= low: + neutral = low + (high - low) / 10.0 + elif high <= 0.0: + neutral = high - (high - low) / 10.0 else: - neutral = 0. + neutral = 0.0 actuator = self._data.actuator(actuator_name) actuator.ctrl = neutral @@ -221,16 +251,16 @@ def _get_actuator_info(self, actuators, control_type): """ if actuators is not None: for actuator in actuators: - self._actuated_joint_names.append(actuator.attrib['joint']) - self.actuator_names.append(actuator.attrib['name']) + self._actuated_joint_names.append(actuator.attrib["joint"]) + self.actuator_names.append(actuator.attrib["name"]) self.control_types.append(control_type) def _get_mimic_joint_info(self, mimic_joint_elements): for name in self._actuated_joint_names: matched = False for element in mimic_joint_elements: - joint1 = element.attrib['joint1'] - joint2 = element.attrib['joint2'] + joint1 = element.attrib["joint1"] + joint2 = element.attrib["joint2"] if joint1 == name: self._mimic_joint_names.append(joint2) matched = True @@ -242,9 +272,9 @@ def _get_actuated_joint_ranges(self, kinematics_root): if kinematics_root is None: return for name in self._actuated_joint_names: - joint = find_elements(kinematics_root, 'joint', {'name': name}) + joint = find_elements(kinematics_root, "joint", {"name": name}) try: - joint_range = string_to_array(joint.attrib['range']) + joint_range = string_to_array(joint.attrib["range"]) self._actuated_joint_ranges[name] = joint_range except KeyError: # Silently handle actuated joints with no range defined, which could be continuous joints @@ -254,11 +284,11 @@ def _get_actuated_joint_geoms(self, kinematics_root): if kinematics_root is None: return for name in self._actuated_joint_names: - joint = find_elements(kinematics_root, 'joint', {'name': name}) + joint = find_elements(kinematics_root, "joint", {"name": name}) try: parent_body = find_parent(kinematics_root, joint) - geom = find_elements(parent_body, 'geom') - geom_name = geom.attrib['name'] + geom = find_elements(parent_body, "geom") + geom_name = geom.attrib["name"] self._actuated_joint_geoms[name] = geom_name except KeyError: self._actuated_joint_geoms[name] = None @@ -281,23 +311,23 @@ def _get_actuator_ranges(self, actuator_root): return for i, name in enumerate(self.actuator_names): if self.control_types[i] == 0: - actuator = find_elements(actuator_root, 'position', {'name': name}) + actuator = find_elements(actuator_root, "position", {"name": name}) elif self.control_types[i] == 1: - actuator = find_elements(actuator_root, 'velocity', {'name': name}) + actuator = find_elements(actuator_root, "velocity", {"name": name}) elif self.control_types[i] == 2: - actuator = find_elements(actuator_root, 'motor', {'name': name}) + actuator = find_elements(actuator_root, "motor", {"name": name}) else: raise NotImplementedError try: - ctrl_range = string_to_array(actuator.attrib['ctrlrange']) + ctrl_range = string_to_array(actuator.attrib["ctrlrange"]) self._actuator_ctrl_ranges[name] = ctrl_range except KeyError: # Silently handle actuators with no ctrl range defined self._actuator_ctrl_ranges[name] = None try: - force_range = string_to_array(actuator.attrib['forcerange']) + force_range = string_to_array(actuator.attrib["forcerange"]) self._actuator_force_ranges[name] = force_range except KeyError: # Silently handle actuators with no ctrl range defined @@ -306,26 +336,35 @@ def _get_actuator_ranges(self, actuator_root): def _get_effort_sensor_info(self, kinematics_root, sensors): if sensors is not None: for sensor in sensors: - site_name = sensor.attrib['site'] - site = find_elements(kinematics_root, 'site', {'name': site_name}) + site_name = sensor.attrib["site"] + site = find_elements(kinematics_root, "site", {"name": site_name}) if site is None: - rospy.logwarn('Site {} is not in the kinematic tree'.format(site_name)) + rospy.logwarn( + "Site {} is not in the kinematic tree".format(site_name) + ) continue parent = find_parent(kinematics_root, site) if parent is not None: - joint = find_elements(parent, 'joint') + joint = find_elements(parent, "joint") if joint is not None: - joint_name = joint.attrib['name'] + joint_name = joint.attrib["name"] try: index = self._actuated_joint_names.index(joint_name) - self.effort_sensor_names[index] = sensor.attrib['name'] + self.effort_sensor_names[index] = sensor.attrib["name"] except ValueError: - rospy.logwarn('Sensor {} corresponds to non-actuated joint {}'.format - (sensor.attrib['name'], joint_name)) + rospy.logwarn( + "Sensor {} corresponds to non-actuated joint {}".format( + sensor.attrib["name"], joint_name + ) + ) else: - rospy.logwarn('Sensor {} have no corresponding joint'.format(sensor.attrib['name'])) + rospy.logwarn( + "Sensor {} have no corresponding joint".format( + sensor.attrib["name"] + ) + ) else: - rospy.logwarn('Cannot find parent for site {}'.format(site_name)) + rospy.logwarn("Cannot find parent for site {}".format(site_name)) def _get_robot_states(self): """Get robot joint states for each step. @@ -343,7 +382,7 @@ def _get_robot_states(self): if self.effort_sensor_names[i] != self._null_sensor: joint_qtau = self._get_effort_sensor_data(self.effort_sensor_names[i]) else: - joint_qtau = 0. + joint_qtau = 0.0 joint_state = [joint_qpos, joint_qvel, joint_qtau] robot_states.append(joint_state) @@ -361,12 +400,18 @@ def _get_robot_states(self): need_update = False if joint_qtau < low: rospy.logwarn( - "{} effort {:.4f} is lower than limit {:.4f}".format(joint_name, joint_qtau, low)) + "{} effort {:.4f} is lower than limit {:.4f}".format( + joint_name, joint_qtau, low + ) + ) level = max(min((low - joint_qtau) / (high - low) * 2, -0.5), -1) need_update = True if joint_qtau > high: rospy.logwarn( - "{} effort {:.4f} is larger than limit {:.4f}".format(joint_name, joint_qtau, high)) + "{} effort {:.4f} is larger than limit {:.4f}".format( + joint_name, joint_qtau, high + ) + ) level = min(max((joint_qtau - high) / (high - low) * 2, 0.5), 1) need_update = True if self.reset_verbose: @@ -378,7 +423,7 @@ def _get_robot_states(self): self.reset_verbose = False self._robot_states = np.array(robot_states) - def _get_effort_sensor_data(self, sensor_name, axis='z'): + def _get_effort_sensor_data(self, sensor_name, axis="z"): """Get the force/effort sensor reading for the given axis. Args: @@ -394,16 +439,22 @@ def _get_effort_sensor_data(self, sensor_name, axis='z'): sensor = self._data.sensor(sensor_name) if len(sensor.data) != 3: rospy.logwarn_throttle( - 1, 'The data number is not 3 for {}, maybe it is not a effort/force sensor?'.format(sensor_name)) + 1, + "The data number is not 3 for {}, maybe it is not a effort/force sensor?".format( + sensor_name + ), + ) return None - if axis.lower() == 'x': + if axis.lower() == "x": return sensor.data[0] - elif axis.lower() == 'y': + elif axis.lower() == "y": return sensor.data[1] - elif axis.lower() == 'z': + elif axis.lower() == "z": return sensor.data[2] else: - raise NotImplementedError('Unsupported axis {}, only x, y, z are valid'.format(axis)) + raise NotImplementedError( + "Unsupported axis {}, only x, y, z are valid".format(axis) + ) def get_joint_states(self): """Convert the robot_states to ROS JointState message. @@ -447,7 +498,9 @@ def get_odom(self): robot_base = self._data.body(self.robot_name) xpos = robot_base.xpos xquat = robot_base.xquat # quaternion, w comes first - cvel = robot_base.cvel # com-based velocity, 6 entities, 3 for rotation, 3 for translation + cvel = ( + robot_base.cvel + ) # com-based velocity, 6 entities, 3 for rotation, 3 for translation odom.header.stamp = rospy.Time().now() pose = to_ros_pose(to_list(xpos) + to_list(xquat), w_first=True) twist = to_ros_twist(to_list(cvel[3:]) + to_list(cvel[:3])) @@ -473,7 +526,7 @@ def get_object_pose(self): except KeyError: return None - def get_site_pose(self, site_name, ref_name=''): + def get_site_pose(self, site_name, ref_name=""): """Get the relative pose of a site with regard to the reference frame. Args: @@ -487,7 +540,7 @@ def get_site_pose(self, site_name, ref_name=''): if self._data is None: return None site_pose = self._get_site_pose(site_name) - if ref_name == '' or ref_name is None: + if ref_name == "" or ref_name is None: ref_pose = self._get_body_pose(self.robot_name) else: ref_pose = self._get_body_pose(ref_name) @@ -506,7 +559,7 @@ def _get_site_pose(self, site_name): xquat = to_ros_orientation(target_site.xmat) return to_ros_pose(to_list(xpos) + to_list(xquat)) except KeyError: - rospy.logerr('Site {} is not exist'.format(site_name)) + rospy.logerr("Site {} is not exist".format(site_name)) return None def _get_body_pose(self, body_name): @@ -518,7 +571,7 @@ def _get_body_pose(self, body_name): xquat = target_body.xquat return to_ros_pose(to_list(xpos) + to_list(xquat), w_first=True) except KeyError: - rospy.logerr('Body {} is not exist'.format(body_name)) + rospy.logerr("Body {} is not exist".format(body_name)) return None def set_joint_command(self, cmd): @@ -529,6 +582,9 @@ def set_joint_command(self, cmd): with the actuator's type. Returns: None + + Raises: + TypeError if the class member control_type is not valid. """ if not cmd.name: for i, actuator_name in enumerate(self.actuator_names): @@ -541,7 +597,10 @@ def set_joint_command(self, cmd): actuator.ctrl = cmd.effort[i] else: raise TypeError( - 'Unsupported control type {}. Valid types are 0, 1, 2'.format(self.control_types[i])) + "Unsupported control type {}. Valid types are 0, 1, 2".format( + self.control_types[i] + ) + ) return for k, name in enumerate(cmd.name): try: @@ -555,26 +614,40 @@ def set_joint_command(self, cmd): actuator.ctrl = cmd.effort[k] else: raise TypeError( - 'Unsupported control type {}. Valid types are 0, 1, 2'.format(self.control_types[i])) + "Unsupported control type {}. Valid types are 0, 1, 2".format( + self.control_types[i] + ) + ) except ValueError: # We allow the name in cmd not present in _actuated_joint_names pass - def set_base_command(self, vel): + def set_base_command(self, vel, factor=1.4): + """Set velocity commands to wheel of the base. + + Args: + vel: list[double] A 4-element list containing velocity commands for the 4 wheels: + FrontLeft, FrontRight, BackLeft, BackRight. + factor: double A value to be multiplied to the vel to migrate the sim-to-real gap. + + Returns: + bool True if succeeded, false otherwise. + """ if len(vel) != 4: - rospy.logwarn_throttle(1, 'Only support 4 velocity commands for wheels') - return - wheel_fl = self._data.actuator('WHEEL_FL') - wheel_fr = self._data.actuator('WHEEL_FR') - wheel_bl = self._data.actuator('WHEEL_BL') - wheel_br = self._data.actuator('WHEEL_BR') - wheel_fl.ctrl, wheel_fr.ctrl, wheel_bl.ctrl, wheel_br.ctrl = vel + rospy.logwarn_throttle(1, "Only support 4 velocity commands for wheels") + return False + wheel_fl = self._data.actuator("WHEEL_FL") + wheel_fr = self._data.actuator("WHEEL_FR") + wheel_bl = self._data.actuator("WHEEL_BL") + wheel_br = self._data.actuator("WHEEL_BR") + wheel_fl.ctrl, wheel_fr.ctrl, wheel_bl.ctrl, wheel_br.ctrl = vel * factor + return True def set_gripper_command(self, joint_names, cmd_values): """Set commands for the gripper containing joints denoted by joint_names. Args: - joint_names: list[str] Names of the joints belonging to the gripper. + joint_names: list[str] Names of the joints belonging to one or more grippers. cmd_values: float/list[float] If a float is given, all joints will be commanded with this value; If a list is given, each of them will be set to a corresponding joint. @@ -586,7 +659,9 @@ def set_gripper_command(self, joint_names, cmd_values): elif isinstance(cmd_values, list) or isinstance(cmd_values, tuple): assert len(joint_names) == len(cmd_values) else: - raise NotImplementedError('cmd_values type {} is not supported'.format(type(cmd_values))) + raise NotImplementedError( + "cmd_values type {} is not supported".format(type(cmd_values)) + ) for joint_name, cmd_value in zip(joint_names, cmd_values): try: @@ -597,11 +672,21 @@ def set_gripper_command(self, joint_names, cmd_values): rospy.logwarn(e) return False + timeout = time.time() + 5.0 # Wait for 5 secs while True: - qpos_list = [self._data.joint(joint_name).qpos.tolist()[0] for joint_name in joint_names] - if all_close(qpos_list, cmd_values, 2.e-3): + qpos_list = [ + self._data.joint(joint_name).qpos.tolist()[0] + for joint_name in joint_names + ] + if all_close(qpos_list, cmd_values, 2.0e-3): + break + if time.time() > timeout: break - rospy.loginfo_throttle(2, "Gripper is reaching to {} (current {})".format(cmd_values, qpos_list)) + rospy.loginfo_throttle( + 2, + "Gripper is reaching to {} (current {})".format(cmd_values, qpos_list), + ) + rospy.sleep(0.1) return True def reset_object_pose(self): @@ -616,7 +701,7 @@ def reset_object_pose(self): return False try: self._model.equality("anchor").active = True - rospy.sleep(1.) + rospy.sleep(1.0) self._model.equality("anchor").active = False return True except KeyError: diff --git a/src/rotools/simulation/mujoco/mujoco_py_interface.py b/src/rotools/simulation/mujoco/mujoco_py_interface.py index 1e54829..7011104 100644 --- a/src/rotools/simulation/mujoco/mujoco_py_interface.py +++ b/src/rotools/simulation/mujoco/mujoco_py_interface.py @@ -8,7 +8,12 @@ from threading import Thread from mujoco_py import MjSim, MjSimState, MjViewer, load_model_from_path -from rotools.utility.mjcf import find_elements, find_parent, array_to_string, string_to_array +from rotools.utility.mjcf import ( + find_elements, + find_parent, + array_to_string, + string_to_array, +) from rotools.utility.common import to_ros_pose, to_ros_twist, to_list try: @@ -26,14 +31,8 @@ class MuJoCoInterface(Thread): - def __init__( - self, - model_path, - kinematics_path, - actuator_path, - enable_viewer=True, - **kwargs + self, model_path, kinematics_path, actuator_path, enable_viewer=True, **kwargs ): """Initialize the MuJoCoInterface. @@ -48,16 +47,22 @@ def __init__( Thread.__init__(self) if not os.path.exists(model_path): - raise FileNotFoundError("Model XML file '{}' does not exist".format(model_path)) + raise FileNotFoundError( + "Model XML file '{}' does not exist".format(model_path) + ) self.model = load_model_from_path(model_path) self.sim = MjSim(self.model) if not os.path.exists(kinematics_path): - raise FileNotFoundError("Kinematics XML file '{}' does not exist".format(kinematics_path)) + raise FileNotFoundError( + "Kinematics XML file '{}' does not exist".format(kinematics_path) + ) if not os.path.exists(actuator_path): - raise FileNotFoundError("Actuator XML file '{}' does not exist".format(actuator_path)) + raise FileNotFoundError( + "Actuator XML file '{}' does not exist".format(actuator_path) + ) kinematic_tree = ElementTree.parse(kinematics_path) kinematics_root = kinematic_tree.getroot() @@ -65,23 +70,27 @@ def __init__( actuator_tree = ElementTree.parse(actuator_path) actuator_root = actuator_tree.getroot() - self.robot_name = find_elements(kinematics_root, 'body').attrib['name'] + self.robot_name = find_elements(kinematics_root, "body").attrib["name"] if self.robot_name is not None: rospy.loginfo("Robot name: {}".format(self.robot_name)) else: raise ValueError("Cannot find body in the kinematic tree") - position_actuators = find_elements(actuator_root, 'position', return_first=False) - velocity_actuators = find_elements(actuator_root, 'velocity', return_first=False) - torque_actuators = find_elements(actuator_root, 'motor', return_first=False) + position_actuators = find_elements( + actuator_root, "position", return_first=False + ) + velocity_actuators = find_elements( + actuator_root, "velocity", return_first=False + ) + torque_actuators = find_elements(actuator_root, "motor", return_first=False) - mimic_joints = find_elements(actuator_root, 'joint', return_first=False) + mimic_joints = find_elements(actuator_root, "joint", return_first=False) - torque_sensors = find_elements(actuator_root, 'torque', return_first=False) - force_sensors = find_elements(actuator_root, 'force', return_first=False) + torque_sensors = find_elements(actuator_root, "torque", return_first=False) + force_sensors = find_elements(actuator_root, "force", return_first=False) # Set the initial state of the simulation (t=0). Assume the keyframe tag is in the actuator xml - initial_keyframe = find_elements(actuator_root, 'key') + initial_keyframe = find_elements(actuator_root, "key") """ control_types: list[int] How the actuator is controlled. @@ -101,22 +110,32 @@ def __init__( self._get_actuated_joint_ranges(kinematics_root) self._actuator_num = len(self.actuator_names) - self._actuator_ids = [self.sim.model.actuator_name2id(actuator_name) for actuator_name in self.actuator_names] + self._actuator_ids = [ + self.sim.model.actuator_name2id(actuator_name) + for actuator_name in self.actuator_names + ] self._wheel_actuator_ids = self._get_wheel_actuator_info() - rospy.loginfo('Controlled joints #{}:\n{}'.format( - self._actuator_num, array_to_string(self._actuated_joint_names)) + rospy.loginfo( + "Controlled joints #{}:\n{}".format( + self._actuator_num, array_to_string(self._actuated_joint_names) + ) ) rospy.loginfo( - 'Control types: position (0), velocity (1), torque (2).\n{}'.format(array_to_string(self.control_types))) + "Control types: position (0), velocity (1), torque (2).\n{}".format( + array_to_string(self.control_types) + ) + ) - self._null_sensor = 'none' + self._null_sensor = "none" self.effort_sensor_names = [self._null_sensor] * self._actuator_num self._get_effort_sensor_info(kinematics_root, torque_sensors) self._get_effort_sensor_info(kinematics_root, force_sensors) - rospy.loginfo('Effort sensors:\n{}'.format(array_to_string(self.effort_sensor_names))) + rospy.loginfo( + "Effort sensors:\n{}".format(array_to_string(self.effort_sensor_names)) + ) self._set_initial_state(initial_keyframe) @@ -124,7 +143,7 @@ def __init__( self._robot_states = None - self._clock_publisher = rospy.Publisher('/clock', Clock, queue_size=1) + self._clock_publisher = rospy.Publisher("/clock", Clock, queue_size=1) @property def n_actuator(self): @@ -143,19 +162,23 @@ def run(self): def _set_initial_state(self, initial_keyframe): if initial_keyframe is not None: - qpos_str = ' '.join(initial_keyframe.attrib['qpos'].split()) + qpos_str = " ".join(initial_keyframe.attrib["qpos"].split()) qpos = string_to_array(qpos_str) if len(qpos) != self.model.nq: - rospy.logwarn('Keyframe qpos size {} does not match internal state {}'.format(len(qpos), self.model.nq)) + rospy.logwarn( + "Keyframe qpos size {} does not match internal state {}".format( + len(qpos), self.model.nq + ) + ) return qvel = np.zeros(self.model.nv) old_state = self.sim.get_state() new_state = MjSimState(0, qpos, qvel, old_state.act, old_state.udd_state) self.sim.set_state(new_state) self.sim.forward() - rospy.loginfo('Set initial state with keyframe values') + rospy.loginfo("Set initial state with keyframe values") else: - rospy.loginfo('No initial state from keyframe values') + rospy.loginfo("No initial state from keyframe values") def _get_actuator_info(self, actuators, control_type): """Given actuator XML elements, get their names and corresponding joint attributes and control types, @@ -170,16 +193,16 @@ def _get_actuator_info(self, actuators, control_type): """ if actuators is not None: for actuator in actuators: - self._actuated_joint_names.append(actuator.attrib['joint']) - self.actuator_names.append(actuator.attrib['name']) + self._actuated_joint_names.append(actuator.attrib["joint"]) + self.actuator_names.append(actuator.attrib["name"]) self.control_types.append(control_type) def _get_mimic_joint_info(self, mimic_joint_elements): for name in self._actuated_joint_names: matched = False for element in mimic_joint_elements: - joint1 = element.attrib['joint1'] - joint2 = element.attrib['joint2'] + joint1 = element.attrib["joint1"] + joint2 = element.attrib["joint2"] if joint1 == name: self._mimic_joint_names.append(joint2) matched = True @@ -189,9 +212,9 @@ def _get_mimic_joint_info(self, mimic_joint_elements): def _get_actuated_joint_ranges(self, kinematics_root): for name in self._actuated_joint_names: - joint = find_elements(kinematics_root, 'joint', {'name': name}) + joint = find_elements(kinematics_root, "joint", {"name": name}) try: - joint_range = string_to_array(joint.attrib['range']) + joint_range = string_to_array(joint.attrib["range"]) self._actuated_joint_ranges[name] = joint_range except KeyError: self._actuated_joint_ranges[name] = None @@ -199,36 +222,52 @@ def _get_actuated_joint_ranges(self, kinematics_root): pass def _get_wheel_actuator_info(self): - wheel_actuator_ids = {'WHEEL_FR': -1, 'WHEEL_FL': -1, 'WHEEL_BL': -1, 'WHEEL_BR': -1} + wheel_actuator_ids = { + "WHEEL_FR": -1, + "WHEEL_FL": -1, + "WHEEL_BL": -1, + "WHEEL_BR": -1, + } for i, name in enumerate(self.actuator_names): if name in wheel_actuator_ids.keys(): wheel_actuator_ids[name] = self._actuator_ids[i] - assert -1 not in wheel_actuator_ids.items(), rospy.logwarn(1, 'Not all wheel actuator found') + assert -1 not in wheel_actuator_ids.items(), rospy.logwarn( + 1, "Not all wheel actuator found" + ) return wheel_actuator_ids def _get_effort_sensor_info(self, kinematics_root, sensors): if sensors is not None: for sensor in sensors: - site_name = sensor.attrib['site'] - site = find_elements(kinematics_root, 'site', {'name': site_name}) + site_name = sensor.attrib["site"] + site = find_elements(kinematics_root, "site", {"name": site_name}) if site is None: - rospy.logwarn('Site {} is not in the kinematic tree'.format(site_name)) + rospy.logwarn( + "Site {} is not in the kinematic tree".format(site_name) + ) continue parent = find_parent(kinematics_root, site) if parent is not None: - joint = find_elements(parent, 'joint') + joint = find_elements(parent, "joint") if joint is not None: - joint_name = joint.attrib['name'] + joint_name = joint.attrib["name"] try: index = self._actuated_joint_names.index(joint_name) - self.effort_sensor_names[index] = sensor.attrib['name'] + self.effort_sensor_names[index] = sensor.attrib["name"] except ValueError: - rospy.logwarn('Sensor {} corresponds to non-actuated joint {}'.format - (sensor.attrib['name'], joint_name)) + rospy.logwarn( + "Sensor {} corresponds to non-actuated joint {}".format( + sensor.attrib["name"], joint_name + ) + ) else: - rospy.logwarn('Sensor {} have no corresponding joint'.format(sensor.attrib['name'])) + rospy.logwarn( + "Sensor {} have no corresponding joint".format( + sensor.attrib["name"] + ) + ) else: - rospy.logwarn('Cannot find parent for site {}'.format(site_name)) + rospy.logwarn("Cannot find parent for site {}".format(site_name)) def _get_robot_states(self): """Get robot joint states for each step. @@ -246,7 +285,7 @@ def _get_robot_states(self): if self.effort_sensor_names[i] != self._null_sensor: joint_effort = self._get_effort_sensor_data(self.effort_sensor_names[i]) else: - joint_effort = 0. + joint_effort = 0.0 joint_state = [joint_qpos, joint_qvel, joint_effort] robot_states.append(joint_state) self._robot_states = np.array(robot_states) @@ -262,11 +301,15 @@ def _get_effort_sensor_data(self, sensor_name): """ sensor_id = self.sim.model.sensor_name2id(sensor_name) try: - effort_data = self.sim.data.sensordata[sensor_id * 3: sensor_id * 3 + 3] - return effort_data[-1] # the force/torque along/around the z-axis is what we care + effort_data = self.sim.data.sensordata[sensor_id * 3 : sensor_id * 3 + 3] + return effort_data[ + -1 + ] # the force/torque along/around the z-axis is what we care except IndexError: - rospy.logerr('Due to a known bug of mujoco-py (see: https://github.com/openai/mujoco-py/issues/684), ' - 'we currently only support sensors with 3 outputs (torque/force)') + rospy.logerr( + "Due to a known bug of mujoco-py (see: https://github.com/openai/mujoco-py/issues/684), " + "we currently only support sensors with 3 outputs (torque/force)" + ) return None def get_joint_states(self): @@ -277,7 +320,7 @@ def get_joint_states(self): None if the robot_state is not available, otherwise return JointState """ if self._robot_states is None: - rospy.logwarn_throttle(1, 'MuJoCo robot state has not been set') + rospy.logwarn_throttle(1, "MuJoCo robot state has not been set") return None joint_state_msg = JointState() joint_state_msg.header.stamp = rospy.Time.now() @@ -324,8 +367,8 @@ def get_object_pose(self): Returns: Pose in the world frame. """ - xpos = self.sim.data.get_body_xpos('object') - xquat = self.sim.data.get_body_xquat('object') + xpos = self.sim.data.get_body_xpos("object") + xquat = self.sim.data.get_body_xquat("object") return to_ros_pose(to_list(xpos) + to_list(xquat), w_first=True) def set_joint_command(self, cmd): @@ -346,7 +389,7 @@ def set_joint_command(self, cmd): elif self.control_types[i] == 2: self.sim.data.ctrl[actuator_id] = cmd.effort[i] else: - raise TypeError('Unsupported type {}'.format(self.control_types[i])) + raise TypeError("Unsupported type {}".format(self.control_types[i])) return for k, name in enumerate(cmd.name): try: @@ -359,20 +402,24 @@ def set_joint_command(self, cmd): elif self.control_types[i] == 2: self.sim.data.ctrl[actuator_id] = cmd.effort[k] else: - raise TypeError('Unsupported type {}'.format(self.control_types[i])) + raise TypeError("Unsupported type {}".format(self.control_types[i])) except ValueError: # We allow the name in cmd not present in _actuated_joint_names pass def set_base_command(self, vel): if len(vel) != 4: - rospy.logwarn('Only support 4 velocity commands for wheels') + rospy.logwarn("Only support 4 velocity commands for wheels") return - self.sim.data.ctrl[[ - self._wheel_actuator_ids['WHEEL_FL'], self._wheel_actuator_ids['WHEEL_FR'], - self._wheel_actuator_ids['WHEEL_BL'], self._wheel_actuator_ids['WHEEL_BR'] - ]] = vel + self.sim.data.ctrl[ + [ + self._wheel_actuator_ids["WHEEL_FL"], + self._wheel_actuator_ids["WHEEL_FR"], + self._wheel_actuator_ids["WHEEL_BL"], + self._wheel_actuator_ids["WHEEL_BR"], + ] + ] = vel def set_gripper_command(self, device_names, value): for device_name in device_names: diff --git a/src/rotools/simulation/mujoco/mujoco_viewer/mujoco_viewer.py b/src/rotools/simulation/mujoco/mujoco_viewer/mujoco_viewer.py index 618fe52..2c7c67f 100644 --- a/src/rotools/simulation/mujoco/mujoco_viewer/mujoco_viewer.py +++ b/src/rotools/simulation/mujoco/mujoco_viewer/mujoco_viewer.py @@ -34,20 +34,17 @@ def __init__(self, model, data): # glfw init glfw.init() width, height = glfw.get_video_mode(glfw.get_primary_monitor()).size - self.window = glfw.create_window( - width // 2, height // 2, "mujoco", None, None) + self.window = glfw.create_window(width // 2, height // 2, "mujoco", None, None) glfw.make_context_current(self.window) glfw.swap_interval(1) - framebuffer_width, framebuffer_height = glfw.get_framebuffer_size( - self.window) + framebuffer_width, framebuffer_height = glfw.get_framebuffer_size(self.window) window_width, _ = glfw.get_window_size(self.window) self._scale = framebuffer_width * 1.0 / window_width # set callbacks glfw.set_cursor_pos_callback(self.window, self._cursor_pos_callback) - glfw.set_mouse_button_callback( - self.window, self._mouse_button_callback) + glfw.set_mouse_button_callback(self.window, self._mouse_button_callback) glfw.set_scroll_callback(self.window, self._scroll_callback) glfw.set_key_callback(self.window, self._key_callback) self._last_left_click_time = None @@ -63,11 +60,11 @@ def __init__(self, model, data): self.scn = mujoco.MjvScene(self.model, maxgeom=10000) self.pert = mujoco.MjvPerturb() self.ctx = mujoco.MjrContext( - self.model, mujoco.mjtFontScale.mjFONTSCALE_150.value) + self.model, mujoco.mjtFontScale.mjFONTSCALE_150.value + ) # get viewport - self.viewport = mujoco.MjrRect( - 0, 0, framebuffer_width, framebuffer_height) + self.viewport = mujoco.MjrRect(0, 0, framebuffer_width, framebuffer_height) # overlay, markers self._overlay = {} @@ -102,9 +99,13 @@ def _key_callback(self, window, key, scancode, action, mods): # Capture screenshot elif key == glfw.KEY_T: img = np.zeros( - (glfw.get_framebuffer_size( - self.window)[1], glfw.get_framebuffer_size( - self.window)[0], 3), dtype=np.uint8) + ( + glfw.get_framebuffer_size(self.window)[1], + glfw.get_framebuffer_size(self.window)[0], + 3, + ), + dtype=np.uint8, + ) mujoco.mjr_readPixels(img, None, self.viewport, self.ctx) imageio.imwrite(self._image_path % self._image_idx, np.flipud(img)) self._image_idx += 1 @@ -130,7 +131,13 @@ def _key_callback(self, window, key, scancode, action, mods): elif key in (glfw.KEY_0, glfw.KEY_1, glfw.KEY_2, glfw.KEY_3, glfw.KEY_4): self.vopt.geomgroup[key - glfw.KEY_0] ^= 1 elif key == glfw.KEY_P: - print("Cam params: ", self.cam.distance, self.cam.azimuth, self.cam.elevation, self.cam.lookat) + print( + "Cam params: ", + self.cam.distance, + self.cam.azimuth, + self.cam.elevation, + self.cam.lookat, + ) # Quit if key == glfw.KEY_ESCAPE: print("Pressed ESC") @@ -143,12 +150,21 @@ def _cursor_pos_callback(self, window, xpos, ypos): return mod_shift = ( - glfw.get_key(window, glfw.KEY_LEFT_SHIFT) == glfw.PRESS or - glfw.get_key(window, glfw.KEY_RIGHT_SHIFT) == glfw.PRESS) + glfw.get_key(window, glfw.KEY_LEFT_SHIFT) == glfw.PRESS + or glfw.get_key(window, glfw.KEY_RIGHT_SHIFT) == glfw.PRESS + ) if self._button_right_pressed: - action = mujoco.mjtMouse.mjMOUSE_MOVE_H if mod_shift else mujoco.mjtMouse.mjMOUSE_MOVE_V + action = ( + mujoco.mjtMouse.mjMOUSE_MOVE_H + if mod_shift + else mujoco.mjtMouse.mjMOUSE_MOVE_V + ) elif self._button_left_pressed: - action = mujoco.mjtMouse.mjMOUSE_ROTATE_H if mod_shift else mujoco.mjtMouse.mjMOUSE_ROTATE_V + action = ( + mujoco.mjtMouse.mjMOUSE_ROTATE_H + if mod_shift + else mujoco.mjtMouse.mjMOUSE_ROTATE_V + ) else: action = mujoco.mjtMouse.mjMOUSE_ZOOM @@ -165,22 +181,23 @@ def _cursor_pos_callback(self, window, xpos, ypos): dx / height, dy / height, self.scn, - self.pert) + self.pert, + ) else: mujoco.mjv_moveCamera( - self.model, - action, - dx / height, - dy / height, - self.scn, - self.cam) + self.model, action, dx / height, dy / height, self.scn, self.cam + ) self._last_mouse_x = int(self._scale * xpos) self._last_mouse_y = int(self._scale * ypos) def _mouse_button_callback(self, window, button, act, mods): - self._button_left_pressed = button == glfw.MOUSE_BUTTON_LEFT and act == glfw.PRESS - self._button_right_pressed = button == glfw.MOUSE_BUTTON_RIGHT and act == glfw.PRESS + self._button_left_pressed = ( + button == glfw.MOUSE_BUTTON_LEFT and act == glfw.PRESS + ) + self._button_right_pressed = ( + button == glfw.MOUSE_BUTTON_RIGHT and act == glfw.PRESS + ) x, y = glfw.get_cursor_pos(window) self._last_mouse_x = int(self._scale * x) @@ -195,7 +212,7 @@ def _mouse_button_callback(self, window, button, act, mods): if self._last_left_click_time is None: self._last_left_click_time = glfw.get_time() - time_diff = (time_now - self._last_left_click_time) + time_diff = time_now - self._last_left_click_time if 0.01 < time_diff < 0.3: self._left_double_click_pressed = True self._last_left_click_time = time_now @@ -204,7 +221,7 @@ def _mouse_button_callback(self, window, button, act, mods): if self._last_right_click_time is None: self._last_right_click_time = glfw.get_time() - time_diff = (time_now - self._last_right_click_time) + time_diff = time_now - self._last_right_click_time if 0.01 < time_diff < 0.2: self._right_double_click_pressed = True self._last_right_click_time = time_now @@ -221,8 +238,7 @@ def _mouse_button_callback(self, window, button, act, mods): # perturbation onste: reset reference if newperturb and not self.pert.active: - mujoco.mjv_initPerturb( - self.model, self.data, self.scn, self.pert) + mujoco.mjv_initPerturb(self.model, self.data, self.scn, self.pert) self.pert.active = newperturb # handle doubleclick @@ -254,7 +270,8 @@ def _mouse_button_callback(self, window, button, act, mods): self.scn, selpnt, selgeom, - selskin) + selskin, + ) # set lookat point, start tracking is requested if selmode == 2 or selmode == 3: @@ -275,8 +292,7 @@ def _mouse_button_callback(self, window, button, act, mods): # compute localpos vec = selpnt.flatten() - self.data.xpos[selbody] mat = self.data.xmat[selbody].reshape(3, 3) - self.pert.localpos = self.data.xmat[selbody].reshape( - 3, 3).dot(vec) + self.pert.localpos = self.data.xmat[selbody].reshape(3, 3).dot(vec) else: self.pert.select = 0 self.pert.skinselect = -1 @@ -290,16 +306,20 @@ def _mouse_button_callback(self, window, button, act, mods): def _scroll_callback(self, window, x_offset, y_offset): with self._gui_lock: mujoco.mjv_moveCamera( - self.model, mujoco.mjtMouse.mjMOUSE_ZOOM, 0, -0.05 * y_offset, self.scn, self.cam) + self.model, + mujoco.mjtMouse.mjMOUSE_ZOOM, + 0, + -0.05 * y_offset, + self.scn, + self.cam, + ) def add_marker(self, **marker_params): self._markers.append(marker_params) def _add_marker_to_scene(self, marker): if self.scn.ngeom >= self.scn.maxgeom: - raise RuntimeError( - 'Ran out of geoms. maxgeom: %d' % - self.scn.maxgeom) + raise RuntimeError("Ran out of geoms. maxgeom: %d" % self.scn.maxgeom) g = self.scn.geoms[self.scn.ngeom] # default values. @@ -335,7 +355,9 @@ def _add_marker_to_scene(self, marker): elif hasattr(g, key): raise ValueError( "mjtGeom has attr {} but type {} is invalid".format( - key, type(value))) + key, type(value) + ) + ) else: raise ValueError("mjtGeom doesn't have field %s" % key) @@ -360,38 +382,28 @@ def add_overlay(gridpos, text1, text2): else: add_overlay( topleft, - "Run speed = %.3f x real time" % - self._run_speed, - "[S]lower, [F]aster") + "Run speed = %.3f x real time" % self._run_speed, + "[S]lower, [F]aster", + ) add_overlay( - topleft, - "Ren[d]er every frame", - "On" if self._render_every_frame else "Off") - add_overlay( - topleft, "Switch camera (#cams = %d)" % - (self.model.ncam + 1), "[Tab] (camera ID = %d)" % - self.cam.fixedcamid) - add_overlay( - topleft, - "[C]ontact forces", - "On" if self._contacts else "Off") + topleft, "Ren[d]er every frame", "On" if self._render_every_frame else "Off" + ) add_overlay( topleft, - "T[r]ansparent", - "On" if self._transparent else "Off") + "Switch camera (#cams = %d)" % (self.model.ncam + 1), + "[Tab] (camera ID = %d)" % self.cam.fixedcamid, + ) + add_overlay(topleft, "[C]ontact forces", "On" if self._contacts else "Off") + add_overlay(topleft, "T[r]ansparent", "On" if self._transparent else "Off") if self._paused is not None: if not self._paused: add_overlay(topleft, "Stop", "[Space]") else: add_overlay(topleft, "Start", "[Space]") - add_overlay( - topleft, - "Advance simulation by one step", - "[right arrow]") + add_overlay(topleft, "Advance simulation by one step", "[right arrow]") add_overlay( - topleft, - "Referenc[e] frames", - "On" if self.vopt.frame == 1 else "Off") + topleft, "Referenc[e] frames", "On" if self.vopt.frame == 1 else "Off" + ) add_overlay(topleft, "[H]ide Menu", "") if self._image_idx > 0: fname = self._image_path % (self._image_idx - 1) @@ -400,16 +412,11 @@ def add_overlay(gridpos, text1, text2): add_overlay(topleft, "Cap[t]ure frame", "") add_overlay(topleft, "Toggle geomgroup visibility", "0-4") + add_overlay(bottomleft, "FPS", "%d%s" % (1 / self._time_per_render, "")) + add_overlay(bottomleft, "Solver iterations", str(self.data.solver_iter + 1)) add_overlay( - bottomleft, "FPS", "%d%s" % - (1 / self._time_per_render, "")) - add_overlay( - bottomleft, "Solver iterations", str( - self.data.solver_iter + 1)) - add_overlay( - bottomleft, "Step", str( - round( - self.data.time / self.model.opt.timestep))) + bottomleft, "Step", str(round(self.data.time / self.model.opt.timestep)) + ) add_overlay(bottomleft, "timestep", "%.5f" % self.model.opt.timestep) def apply_perturbations(self): @@ -430,7 +437,8 @@ def update(): glfw.terminate() sys.exit(0) self.viewport.width, self.viewport.height = glfw.get_framebuffer_size( - self.window) + self.window + ) with self._gui_lock: # update scene mujoco.mjv_updateScene( @@ -440,7 +448,8 @@ def update(): self.pert, self.cam, mujoco.mjtCatBit.mjCAT_ALL.value, - self.scn) + self.scn, + ) # marker items for marker in self._markers: self._add_marker_to_scene(marker) @@ -455,11 +464,13 @@ def update(): self.viewport, t1, t2, - self.ctx) + self.ctx, + ) glfw.swap_buffers(self.window) glfw.poll_events() - self._time_per_render = 0.9 * self._time_per_render + \ - 0.1 * (time.time() - render_start) + self._time_per_render = 0.9 * self._time_per_render + 0.1 * ( + time.time() - render_start + ) # clear overlay self._overlay.clear() @@ -471,8 +482,9 @@ def update(): self._advance_by_one_step = False break else: - self._loop_count += self.model.opt.timestep / \ - (self._time_per_render * self._run_speed) + self._loop_count += self.model.opt.timestep / ( + self._time_per_render * self._run_speed + ) if self._render_every_frame: self._loop_count = 1 while self._loop_count > 0: diff --git a/src/rotools/simulation/mujoco/server.py b/src/rotools/simulation/mujoco/server.py index dfd4b10..f1aeb5a 100644 --- a/src/rotools/simulation/mujoco/server.py +++ b/src/rotools/simulation/mujoco/server.py @@ -30,53 +30,82 @@ def __init__(self, kwargs): self.interface.start() # Provided services - self._execute_gripper_control_srv = rospy.Service("execute_gripper_control", ExecuteBinaryAction, - self.execute_gripper_control_handle) - - self._execute_object_reset_srv = rospy.Service("execute_object_reset", ExecuteBinaryAction, - self.execute_object_reset_handle) - - self._execute_verbose_reset_srv = rospy.Service("execute_verbose_reset", ExecuteBinaryAction, - self.execute_verbose_reset_handle) - - self._get_pose_srv = rospy.Service("get_group_pose", GetGroupPose, self.get_pose_handle) + self._execute_gripper_control_srv = rospy.Service( + "execute_gripper_control", + ExecuteBinaryAction, + self.execute_gripper_control_handle, + ) + + self._execute_object_reset_srv = rospy.Service( + "execute_object_reset", + ExecuteBinaryAction, + self.execute_object_reset_handle, + ) + + self._execute_verbose_reset_srv = rospy.Service( + "execute_verbose_reset", + ExecuteBinaryAction, + self.execute_verbose_reset_handle, + ) + + self._get_pose_srv = rospy.Service( + "get_group_pose", GetGroupPose, self.get_pose_handle + ) # Received msgs - if 'joint_command_topic_id' in kwargs.keys(): - joint_command_topic_id = kwargs['joint_command_topic_id'] - self.joint_command_subscriber = rospy.Subscriber(joint_command_topic_id, JointState, self.joint_command_cb) - - if 'base_command_topic_id' in kwargs.keys(): - base_command_topic_id = kwargs['base_command_topic_id'] - self.base_command_subscriber = rospy.Subscriber(base_command_topic_id, Twist, self.base_command_cb) + if "joint_command_topic_id" in kwargs.keys(): + joint_command_topic_id = kwargs["joint_command_topic_id"] + self.joint_command_subscriber = rospy.Subscriber( + joint_command_topic_id, JointState, self.joint_command_cb + ) + + if "base_command_topic_id" in kwargs.keys(): + base_command_topic_id = kwargs["base_command_topic_id"] + self.base_command_subscriber = rospy.Subscriber( + base_command_topic_id, Twist, self.base_command_cb + ) # Published msgs - rate = kwargs['publish_rate'] if 'publish_rate' in kwargs.keys() else 60 + rate = kwargs["publish_rate"] if "publish_rate" in kwargs.keys() else 60 - if 'joint_state_topic_id' in kwargs.keys(): - joint_state_topic_id = kwargs['joint_state_topic_id'] - self.joint_state_publisher = rospy.Publisher(joint_state_topic_id, JointState, queue_size=1) + if "joint_state_topic_id" in kwargs.keys(): + joint_state_topic_id = kwargs["joint_state_topic_id"] + self.joint_state_publisher = rospy.Publisher( + joint_state_topic_id, JointState, queue_size=1 + ) - if 'odom_topic_id' in kwargs.keys(): - odom_topic_id = kwargs['odom_topic_id'] + if "odom_topic_id" in kwargs.keys(): + odom_topic_id = kwargs["odom_topic_id"] self.odom_publisher = rospy.Publisher(odom_topic_id, Odometry, queue_size=1) else: self.odom_publisher = None - if 'object_pose_topic_id' in kwargs.keys(): - object_pose_topic_id = kwargs['object_pose_topic_id'] - self.object_pose_publisher = rospy.Publisher(object_pose_topic_id, Pose, queue_size=1) + if "object_pose_topic_id" in kwargs.keys(): + object_pose_topic_id = kwargs["object_pose_topic_id"] + self.object_pose_publisher = rospy.Publisher( + object_pose_topic_id, Pose, queue_size=1 + ) else: self.object_pose_publisher = None - self._timer = rospy.Timer(rospy.Duration.from_sec(1.0 / rate), self.publish_handle) - if kwargs['verbose']: - self._verbose_reset_timer = rospy.Timer(rospy.Duration.from_sec(5), self.execute_verbose_reset_handle) + self._timer = rospy.Timer( + rospy.Duration.from_sec(1.0 / rate), self.publish_handle + ) + if kwargs["verbose"]: + self._verbose_reset_timer = rospy.Timer( + rospy.Duration.from_sec(5), self.execute_verbose_reset_handle + ) # Get robot base params if available - self.wheel_radius = kwargs['wheel_radius'] if 'wheel_radius' in kwargs.keys() else None - self.base_width = kwargs['base_width'] if 'base_width' in kwargs.keys() else None - self.base_length = kwargs['base_length'] if 'base_length' in kwargs.keys() else None + self.wheel_radius = ( + kwargs["wheel_radius"] if "wheel_radius" in kwargs.keys() else None + ) + self.base_width = ( + kwargs["base_width"] if "base_width" in kwargs.keys() else None + ) + self.base_length = ( + kwargs["base_length"] if "base_length" in kwargs.keys() else None + ) def publish_handle(self, _): joint_state_msg = self.interface.get_joint_states() @@ -94,25 +123,45 @@ def publish_handle(self, _): self.object_pose_publisher.publish(object_pose_msg) def joint_command_cb(self, cmd): - if len(cmd.position) != len(cmd.velocity) or len(cmd.position) != len(cmd.effort): - rospy.logwarn_throttle(3, 'Joint command should contain position, velocity, and effort. ' - 'Their dimensions should be the same') + if len(cmd.position) != len(cmd.velocity) or len(cmd.position) != len( + cmd.effort + ): + rospy.logwarn_throttle( + 3, + "Joint command should contain position, velocity, and effort. " + "Their dimensions should be the same", + ) return if not cmd.name: - rospy.logwarn_throttle(3, 'Sending joint command with no name is highly discouraged') + rospy.logwarn_throttle( + 3, "Sending joint command with no name is highly discouraged" + ) if len(cmd.position) != self.interface.n_actuator: - rospy.logerr('Joint command size {} and actuator number {} mismatch, omitted.'.format( - len(cmd.position), self.interface.n_actuator)) + rospy.logerr( + "Joint command size {} and actuator number {} mismatch, omitted.".format( + len(cmd.position), self.interface.n_actuator + ) + ) return self.interface.set_joint_command(cmd) def base_command_cb(self, cmd): - if self.wheel_radius is None or self.base_width is None or self.base_length is None: - rospy.logwarn_throttle(1, 'Base param not set: wheel_radius {}, base_width {}, base_length {}'.format( - self.wheel_radius, self.base_width, self.base_length)) + if ( + self.wheel_radius is None + or self.base_width is None + or self.base_length is None + ): + rospy.logwarn_throttle( + 1, + "Base param not set: wheel_radius {}, base_width {}, base_length {}".format( + self.wheel_radius, self.base_width, self.base_length + ), + ) return - vel = mecanum_base_get_wheel_velocities(cmd, self.wheel_radius, self.base_width, self.base_length) + vel = mecanum_base_get_wheel_velocities( + cmd, self.wheel_radius, self.base_width, self.base_length + ) self.interface.set_base_command(vel) def execute_gripper_control_handle(self, req): diff --git a/src/rotools/simulation/webots.py b/src/rotools/simulation/webots.py index 8dc5b4a..3ff8db4 100644 --- a/src/rotools/simulation/webots.py +++ b/src/rotools/simulation/webots.py @@ -17,19 +17,25 @@ def __init__(self): super(WebotsInterfaceUR, self).__init__() self.robot = Supervisor() - print('Robot name: ', self.robot.getName()) + print("Robot name: ", self.robot.getName()) # To get TCP pose, we need add a Transform to the children slot # of the last endPoint's children->Transform->children, # and change the DEF name of the Transform to UR10_TCP - self.base_node = self.robot.getFromDef('UR_BASE') - self.eef_node = self.robot.getFromDef('UR_TCP') + self.base_node = self.robot.getFromDef("UR_BASE") + self.eef_node = self.robot.getFromDef("UR_TCP") self.time_step = int(self.robot.getBasicTimeStep()) # modify this according to robot configuration - self.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', - 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] + self.joint_names = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ] self.motors = [] for name in self.joint_names: motor = self.robot.getMotor(name) @@ -92,9 +98,7 @@ def go_to_joint_state(self, joint_goal, tolerance=1e-4): break def _set_joint_position_sensor(self, enabled=True): - """Enable/disable position sensors of the joints. - - """ + """Enable/disable position sensors of the joints.""" for motor in self.motors: try: js_sensor = motor.getPositionSensor() @@ -103,9 +107,7 @@ def _set_joint_position_sensor(self, enabled=True): pass def set_joint_max_velocity(self, velocity): - """ - - """ + """ """ if isinstance(velocity, list): for i, vel in enumerate(velocity): try: diff --git a/src/rotools/snapshot/core/interface.py b/src/rotools/snapshot/core/interface.py index ca54e96..30121b5 100644 --- a/src/rotools/snapshot/core/interface.py +++ b/src/rotools/snapshot/core/interface.py @@ -16,29 +16,21 @@ class SnapshotInterface(object): - - def __init__( - self, - js_topics, - odom_topics, - pose_topics, - save_dir, - **kwargs - ): + def __init__(self, js_topics, odom_topics, pose_topics, save_dir, **kwargs): super(SnapshotInterface, self).__init__() self._entities = {} if not os.path.isdir(save_dir): - self.save_dir = '/tmp' + self.save_dir = "/tmp" else: os.makedirs(save_dir, exist_ok=True) self.save_dir = save_dir rospy.loginfo("Saving snapshots to {}".format(self.save_dir)) - self._make_csv_entity(js_topics, 'JS_') - self._make_csv_entity(odom_topics, 'ODOM_') - self._make_csv_entity(pose_topics, 'POSE_') + self._make_csv_entity(js_topics, "JS_") + self._make_csv_entity(odom_topics, "ODOM_") + self._make_csv_entity(pose_topics, "POSE_") self._bridge = CvBridge() @@ -46,13 +38,17 @@ def _make_csv_entity(self, topics, prefix): if topics is None: return for topic in topics: - assert isinstance(topic, str), print("Topic type is not str ({})".format(type(topic))) - file_name = prefix + time.strftime('%H%M%S') + topic.replace('/', '_') + '.csv' + assert isinstance(topic, str), print( + "Topic type is not str ({})".format(type(topic)) + ) + file_name = ( + prefix + time.strftime("%H%M%S") + topic.replace("/", "_") + ".csv" + ) file_path = os.path.join(self.save_dir, file_name) entity = [file_path, False] self._entities[topic] = entity - def save_joint_state_msg(self, topic, msg, position_only, tag=''): + def save_joint_state_msg(self, topic, msg, position_only, tag=""): assert isinstance(msg, JointState), print(type(msg)) if topic not in self._entities: rospy.logerr("The interface does not hold the topic {}".format(topic)) @@ -60,20 +56,22 @@ def save_joint_state_msg(self, topic, msg, position_only, tag=''): file_path, has_header = self._entities[topic] if not os.path.exists(file_path): - has_header = False # In case the file is removed while the program is still running + has_header = ( + False # In case the file is removed while the program is still running + ) - with open(file_path, 'a', newline='') as f: + with open(file_path, "a", newline="") as f: writer = csv.writer(f) if not has_header: - writer.writerow(['tag'] + msg.name) + writer.writerow(["tag"] + msg.name) self._entities[topic] = [file_path, True] - writer.writerow(['q_' + str(tag)] + self.to_str_list(msg.position)) + writer.writerow(["q_" + str(tag)] + self.to_str_list(msg.position)) if not position_only: - writer.writerow(['dq_' + str(tag)] + self.to_str_list(msg.velocity)) - writer.writerow(['tau_' + str(tag)] + self.to_str_list(msg.effort)) + writer.writerow(["dq_" + str(tag)] + self.to_str_list(msg.velocity)) + writer.writerow(["tau_" + str(tag)] + self.to_str_list(msg.effort)) return True - def save_odom_msg(self, topic, msg, tag=''): + def save_odom_msg(self, topic, msg, tag=""): assert isinstance(msg, Odometry), print(type(msg)) if topic not in self._entities: rospy.logerr("The interface does not hold the topic {}".format(topic)) @@ -83,17 +81,22 @@ def save_odom_msg(self, topic, msg, tag=''): if not os.path.exists(file_path): has_header = False - with open(file_path, 'a', newline='') as f: + with open(file_path, "a", newline="") as f: writer = csv.writer(f) if not has_header: - writer.writerow(['tag', 'p_x', 'p_y', 'p_z', 'o_x', 'o_y', 'o_z', 'o_w']) + writer.writerow( + ["tag", "p_x", "p_y", "p_z", "o_x", "o_y", "o_z", "o_w"] + ) self._entities[topic] = [file_path, True] p = msg.pose.pose.position o = msg.pose.pose.orientation - writer.writerow(['pose_' + str(tag)] + self.to_str_list([p.x, p.y, p.z, o.x, o.y, o.z, o.w])) + writer.writerow( + ["pose_" + str(tag)] + + self.to_str_list([p.x, p.y, p.z, o.x, o.y, o.z, o.w]) + ) return True - def save_pose_msg(self, topic, msg, tag=''): + def save_pose_msg(self, topic, msg, tag=""): if isinstance(msg, PoseStamped): return save_pose_msg(topic, msg.pose, tag) @@ -106,30 +109,39 @@ def save_pose_msg(self, topic, msg, tag=''): if not os.path.exists(file_path): has_header = False - with open(file_path, 'a', newline='') as f: + with open(file_path, "a", newline="") as f: writer = csv.writer(f) if not has_header: - writer.writerow(['tag', 'p_x', 'p_y', 'p_z', 'o_x', 'o_y', 'o_z', 'o_w']) + writer.writerow( + ["tag", "p_x", "p_y", "p_z", "o_x", "o_y", "o_z", "o_w"] + ) self._entities[topic] = [file_path, True] p = msg.position o = msg.orientation - writer.writerow(['pose_' + str(tag)] + self.to_str_list([p.x, p.y, p.z, o.x, o.y, o.z, o.w])) + writer.writerow( + ["pose_" + str(tag)] + + self.to_str_list([p.x, p.y, p.z, o.x, o.y, o.z, o.w]) + ) return True - def save_image_msgs(self, rgb_topic, depth_topic, rgb_msg, depth_msg, tag=''): - prefix = time.strftime('%H%M%S') + tag + '_' - rgb_ok = self._save_image_msg(rgb_topic, rgb_msg, prefix, '.jpg') - depth_ok = self._save_image_msg(depth_topic, depth_msg, prefix, '.png') + def save_image_msgs(self, rgb_topic, depth_topic, rgb_msg, depth_msg, tag=""): + prefix = time.strftime("%H%M%S") + tag + "_" + rgb_ok = self._save_image_msg(rgb_topic, rgb_msg, prefix, ".jpg") + depth_ok = self._save_image_msg(depth_topic, depth_msg, prefix, ".png") return rgb_ok | depth_ok - def _save_image_msg(self, topic, msg, prefix='', suffix=''): - file_name = prefix + topic.replace('/', '_') + suffix + def _save_image_msg(self, topic, msg, prefix="", suffix=""): + file_name = prefix + topic.replace("/", "_") + suffix if isinstance(msg, Image): - cv_image = self._bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') + cv_image = self._bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough") elif isinstance(msg, CompressedImage): - cv_image = self._bridge.compressed_imgmsg_to_cv2(msg, desired_encoding='passthrough') + cv_image = self._bridge.compressed_imgmsg_to_cv2( + msg, desired_encoding="passthrough" + ) else: - raise NotImplementedError("Image type {} is not implemented".format(type(msg))) + raise NotImplementedError( + "Image type {} is not implemented".format(type(msg)) + ) cv2.imwrite(os.path.join(self.save_dir, file_name), cv_image) return True @@ -137,6 +149,6 @@ def _save_image_msg(self, topic, msg, prefix='', suffix=''): def to_str_list(values, precision=5): output = [] for value in values: - value_str = '{:.{}f}'.format(value, precision) + value_str = "{:.{}f}".format(value, precision) output.append(value_str) return output diff --git a/src/rotools/snapshot/core/server.py b/src/rotools/snapshot/core/server.py index 84d5493..5080fb3 100644 --- a/src/rotools/snapshot/core/server.py +++ b/src/rotools/snapshot/core/server.py @@ -25,19 +25,37 @@ def __init__(self, kwargs): self._subscribers = [] self._msg_dict = {} - kwargs['js_topics'] = self.register_topics(kwargs['js_topics'], sensor_msgs.msg.JointState) - kwargs['odom_topics'] = self.register_topics(kwargs['odom_topics'], nav_msgs.msg.Odometry) - kwargs['pose_topics'] = self.register_topics(kwargs['pose_topics'], geometry_msgs.msg.Pose) - - self.register_topics(kwargs['rgb_compressed_topics'], sensor_msgs.msg.CompressedImage) - self.register_topics(kwargs['depth_compressed_topics'], sensor_msgs.msg.CompressedImage) + kwargs["js_topics"] = self.register_topics( + kwargs["js_topics"], sensor_msgs.msg.JointState + ) + kwargs["odom_topics"] = self.register_topics( + kwargs["odom_topics"], nav_msgs.msg.Odometry + ) + kwargs["pose_topics"] = self.register_topics( + kwargs["pose_topics"], geometry_msgs.msg.Pose + ) + + self.register_topics( + kwargs["rgb_compressed_topics"], sensor_msgs.msg.CompressedImage + ) + self.register_topics( + kwargs["depth_compressed_topics"], sensor_msgs.msg.CompressedImage + ) self.interface = interface.SnapshotInterface(**kwargs) - self._srv_save_joint_state = rospy.Service('save_joint_state', SaveJointState, self.save_joint_state_handle) - self._srv_save_odom = rospy.Service('save_odom', SaveOdometry, self.save_odom_handle) - self._srv_save_pose = rospy.Service('save_pose', SavePose, self.save_pose_handle) - self._srv_save_image = rospy.Service('save_image', SaveImage, self.save_image_handle) + self._srv_save_joint_state = rospy.Service( + "save_joint_state", SaveJointState, self.save_joint_state_handle + ) + self._srv_save_odom = rospy.Service( + "save_odom", SaveOdometry, self.save_odom_handle + ) + self._srv_save_pose = rospy.Service( + "save_pose", SavePose, self.save_pose_handle + ) + self._srv_save_image = rospy.Service( + "save_image", SaveImage, self.save_image_handle + ) def register_topics(self, topics, topic_type): """Register subscribers for cared topics. @@ -54,16 +72,27 @@ def register_topics(self, topics, topic_type): for topic in topics: real_type, _, _ = rostopic.get_topic_type(topic, blocking=False) if real_type is None: - rospy.logwarn("Topic {} has not been published, type check skipped".format(topic)) + rospy.logwarn( + "Topic {} has not been published, type check skipped".format( + topic + ) + ) else: type_name = topic_type.__name__ - if type_name != real_type and type_name != real_type.split('/')[-1]: - rospy.logerr("Topic {} assigned as {}, but the real type is {}, omitted".format( - topic, type_name, real_type)) + if type_name != real_type and type_name != real_type.split("/")[-1]: + rospy.logerr( + "Topic {} assigned as {}, but the real type is {}, omitted".format( + topic, type_name, real_type + ) + ) continue - subscriber = rospy.Subscriber(topic, topic_type, self.msg_cb, topic, queue_size=1) + subscriber = rospy.Subscriber( + topic, topic_type, self.msg_cb, topic, queue_size=1 + ) self._subscribers.append(subscriber) - rospy.loginfo("Registered topic {} of type {}".format(topic, topic_type.__name__)) + rospy.loginfo( + "Registered topic {} of type {}".format(topic, topic_type.__name__) + ) valid_topics.append(topic) return valid_topics else: @@ -80,7 +109,9 @@ def save_image_handle(self, req): else: depth_msg = None - ok = self.interface.save_image_msgs(req.rgb_topic, req.depth_topic, rgb_msg, depth_msg, req.tag) + ok = self.interface.save_image_msgs( + req.rgb_topic, req.depth_topic, rgb_msg, depth_msg, req.tag + ) resp.result_status = resp.SUCCEEDED if ok else resp.FAILED return resp @@ -88,10 +119,14 @@ def save_joint_state_handle(self, req): resp = SaveJointStateResponse() if req.topic in self._msg_dict: msg = self._msg_dict[req.topic] - ok = self.interface.save_joint_state_msg(req.topic, msg, req.position_only, req.tag) + ok = self.interface.save_joint_state_msg( + req.topic, msg, req.position_only, req.tag + ) resp.result_status = resp.SUCCEEDED if ok else resp.FAILED else: - rospy.logwarn("Joint state topic {} has not been recorded".format(req.topic)) + rospy.logwarn( + "Joint state topic {} has not been recorded".format(req.topic) + ) resp.result_status = resp.FAILED return resp diff --git a/src/rotools/tests/test_robot_model.py b/src/rotools/tests/test_robot_model.py index 30e001b..9b79ada 100644 --- a/src/rotools/tests/test_robot_model.py +++ b/src/rotools/tests/test_robot_model.py @@ -12,7 +12,6 @@ class Test(unittest.TestCase): - def test_panda_model_fk(self): mdh, q_limits = panda_mdh_model() panda_mdh = serial_model.RobotModel.get_model_from_mdh(mdh, q_limits) @@ -64,8 +63,12 @@ def test_ur10e_model_mdh_to_poe(self): ur10e_poe = serial_model.RobotModel.get_model_from_poe(poe) ur10e_poe.q_limits = q_limits - self.assertTrue(common.all_close(ur10e_mdh.poe.home_matrix, ur10e_poe.poe.home_matrix)) - self.assertTrue(common.all_close(ur10e_mdh.poe.screw_axes, ur10e_poe.poe.screw_axes)) + self.assertTrue( + common.all_close(ur10e_mdh.poe.home_matrix, ur10e_poe.poe.home_matrix) + ) + self.assertTrue( + common.all_close(ur10e_mdh.poe.screw_axes, ur10e_poe.poe.screw_axes) + ) def test_panda_model_mdh_to_poe(self): mdh, q_limits = panda_mdh_model() @@ -74,14 +77,22 @@ def test_panda_model_mdh_to_poe(self): poe, q_limits = panda_poe_model() panda_poe = serial_model.RobotModel.get_model_from_poe(poe) - self.assertTrue(common.all_close(panda_mdh.poe.home_matrix, panda_poe.poe.home_matrix)) - self.assertTrue(common.all_close(panda_mdh.poe.screw_axes, panda_poe.poe.screw_axes)) + self.assertTrue( + common.all_close(panda_mdh.poe.home_matrix, panda_poe.poe.home_matrix) + ) + self.assertTrue( + common.all_close(panda_mdh.poe.screw_axes, panda_poe.poe.screw_axes) + ) def test_ur10e_model_jacobian_space(self): poe, q_limits = ur10e_poe_model() ur10e_poe = serial_model.RobotModel.get_model_from_poe(poe, q_limits) - self.assertTrue(common.all_close(ur10e_poe.poe.screw_axes, ur10e_poe.jacobian_space(ur10e_poe.q0))) + self.assertTrue( + common.all_close( + ur10e_poe.poe.screw_axes, ur10e_poe.jacobian_space(ur10e_poe.q0) + ) + ) ur10e_poe.jacobian_space(ur10e_poe.random_valid_q()) # def test_create_walker_arm_model(self): @@ -136,5 +147,5 @@ def test_ur10e_model_jacobian_space(self): # print(l1_to_l6) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/src/rotools/tests/test_utility_common.py b/src/rotools/tests/test_utility_common.py index 406b72f..a607494 100644 --- a/src/rotools/tests/test_utility_common.py +++ b/src/rotools/tests/test_utility_common.py @@ -15,13 +15,12 @@ class Test(unittest.TestCase): - def test_print(self): - common.print_debug('This is a debug msg') - common.print_info('This is a info msg') - common.print_warn('This is a warn msg') - common.print_warning('This is a warning msg') - common.print_error('This is a error msg') + common.print_debug("This is a debug msg") + common.print_info("This is a info msg") + common.print_warn("This is a warn msg") + common.print_warning("This is a warning msg") + common.print_error("This is a error msg") def test_all_close(self): values_a = [1, 2, 4] @@ -85,10 +84,10 @@ def test_relative_transform(self): """ pose_1 = common.sd_pose(np.array([0.307, 0, 0.59, 0.924, -0.382, 0, 0])) pose_2 = common.sd_pose(np.array([0.307, 0, 0.59, -0.708, 0.706, 0, 0])) - rel_trans = common.sd_pose(np.array([0, 0, 0, 0., 0., 0.383, 0.924])) + rel_trans = common.sd_pose(np.array([0, 0, 0, 0.0, 0.0, 0.383, 0.924])) trans = np.dot(pose_1, rel_trans) ros_pose_2 = common.to_ros_pose(pose_2) - print(trans, '\n', pose_2, '\n', ros_pose_2) + print(trans, "\n", pose_2, "\n", ros_pose_2) self.assertTrue(common.all_close(common.to_ros_pose(trans), ros_pose_2, 0.01)) # def test_create_publishers(self): @@ -112,5 +111,5 @@ def test_to_ros_orientation(self): common.to_ros_orientation([0, 0, 0, 0], check=True) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/src/rotools/tests/test_utility_robotics.py b/src/rotools/tests/test_utility_robotics.py index efb2df0..82c003a 100644 --- a/src/rotools/tests/test_utility_robotics.py +++ b/src/rotools/tests/test_utility_robotics.py @@ -13,28 +13,35 @@ class Test(unittest.TestCase): - def test_adjoint_T(self): - T = np.array([[1, 0, 0, 0], - [0, 0, -1, 0], - [0, 1, 0, 3], - [0, 0, 0, 1]], dtype=float) - Ad_T = np.array([[1, 0, 0, 0, 0, 0], - [0, 0, -1, 0, 0, 0], - [0, 1, 0, 0, 0, 0], - [0, 0, 3, 1, 0, 0], - [3, 0, 0, 0, 0, -1], - [0, 0, 0, 0, 1, 0]], dtype=float) + T = np.array( + [[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 1]], dtype=float + ) + Ad_T = np.array( + [ + [1, 0, 0, 0, 0, 0], + [0, 0, -1, 0, 0, 0], + [0, 1, 0, 0, 0, 0], + [0, 0, 3, 1, 0, 0], + [3, 0, 0, 0, 0, -1], + [0, 0, 0, 0, 1, 0], + ], + dtype=float, + ) self.assertTrue(np.allclose(robotics.Adjoint(T), Ad_T)) def test_adjoint_V(self): V = np.array([1, 2, 3, 4, 5, 6]) - ad_V = np.array([[0, -3, 2, 0, 0, 0], - [3, 0, -1, 0, 0, 0], - [-2, 1, 0, 0, 0, 0], - [0, -6, 5, 0, -3, 2], - [6, 0, -4, 3, 0, -1], - [-5, 4, 0, -2, 1, 0]]) + ad_V = np.array( + [ + [0, -3, 2, 0, 0, 0], + [3, 0, -1, 0, 0, 0], + [-2, 1, 0, 0, 0, 0], + [0, -6, 5, 0, -3, 2], + [6, 0, -4, 3, 0, -1], + [-5, 4, 0, -2, 1, 0], + ] + ) self.assertTrue(np.allclose(robotics.ad(V), ad_V)) def test_axis_angle3(self): @@ -50,5 +57,5 @@ def test_axis_angle6(self): self.assertTrue(np.allclose(robotics.axis_angle6(exp_c6)[1], result[1])) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/src/rotools/tests/test_utility_transform.py b/src/rotools/tests/test_utility_transform.py index b3947b6..af84c9b 100644 --- a/src/rotools/tests/test_utility_transform.py +++ b/src/rotools/tests/test_utility_transform.py @@ -27,21 +27,27 @@ def test_transform(self): 90 * np.pi / 180, (0, 1, 0) [0. 0.70710678 0. 0.70710678] -90 * np.pi / 180, (0, 1, 0) [0. -0.70710678 0. 0.70710678] """ - homogeneous_matrix = np.array([ - [0., 0., 1., 0.084415], - [1., 0., 0., 0.], - [0., 1., 0, 0.098503 + 0.093313], - [0., 0., 0., 1.] - ], dtype=float) + homogeneous_matrix = np.array( + [ + [0.0, 0.0, 1.0, 0.084415], + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0, 0.098503 + 0.093313], + [0.0, 0.0, 0.0, 1.0], + ], + dtype=float, + ) q = transform.quaternion_from_matrix(homogeneous_matrix) # walker base link to head l3 - homogeneous_matrix = np.array([ - [0., 0., 1., 0.084415], - [1., 0., 0., 0.], - [0., 1., 0, 0.098503 + 0.093313], - [0., 0., 0., 1.] - ], dtype=float) + homogeneous_matrix = np.array( + [ + [0.0, 0.0, 1.0, 0.084415], + [1.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0, 0.098503 + 0.093313], + [0.0, 0.0, 0.0, 1.0], + ], + dtype=float, + ) q = transform.quaternion_from_matrix(homogeneous_matrix) print("base_link to l3\n", q) @@ -54,30 +60,46 @@ def test_transform(self): 270 deg: [ 0. 0. -0.70710678 0.70710678] """ # Franka robot install on non-default pose (curiosity's left arm) - qm_left = transform.quaternion_matrix([-0.40318, 0.52543, 0.097796, 0.74283]) # T_base_to_install - print('qm_left\n', qm_left) - g_vector = np.array([0., 0., -9.81, 0]) # G_base - g_vector_t = np.dot(qm_left.transpose(), g_vector) # G_install = T_install_to_base * G_base + qm_left = transform.quaternion_matrix( + [-0.40318, 0.52543, 0.097796, 0.74283] + ) # T_base_to_install + print("qm_left\n", qm_left) + g_vector = np.array([0.0, 0.0, -9.81, 0]) # G_base + g_vector_t = np.dot( + qm_left.transpose(), g_vector + ) # G_install = T_install_to_base * G_base print(qm_left.transpose()) - print('left arm g_vector_t\n', g_vector_t) + print("left arm g_vector_t\n", g_vector_t) # Franka robot install on non-default pose (curiosity's right arm) - qm_right = transform.quaternion_matrix([0.40318, 0.52543, -0.097796, 0.74283]) # T_base_to_install - g_vector = np.array([0., 0., -9.81, 0]) # G_base - g_vector_t = np.dot(qm_right.transpose(), g_vector) # G_install = T_install_to_base * G_base - print('right arm g_vector_t\n', g_vector_t) + qm_right = transform.quaternion_matrix( + [0.40318, 0.52543, -0.097796, 0.74283] + ) # T_base_to_install + g_vector = np.array([0.0, 0.0, -9.81, 0]) # G_base + g_vector_t = np.dot( + qm_right.transpose(), g_vector + ) # G_install = T_install_to_base * G_base + print("right arm g_vector_t\n", g_vector_t) print(np.linalg.norm(np.subtract(g_vector_t, g_vector))) # Calculate the orientation of the CURI arms regarding the base frame qm_left_new = transform.quaternion_matrix( - [-0.5824349629454723, 0.3718840519259553, 0.2206752352096998, 0.688312549534299]) + [ + -0.5824349629454723, + 0.3718840519259553, + 0.2206752352096998, + 0.688312549534299, + ] + ) print(qm_left_new) print(np.rad2deg(transform.euler_from_matrix(qm_left_new))) # FR Wheel joint axis - rotated_axis = np.dot(transform.euler_matrix(0, np.deg2rad(-30), np.deg2rad(-45), 'rxyz'), - np.array([0., 1., 0., 0.])) + rotated_axis = np.dot( + transform.euler_matrix(0, np.deg2rad(-30), np.deg2rad(-45), "rxyz"), + np.array([0.0, 1.0, 0.0, 0.0]), + ) print(rotated_axis) def test_quaternion_multiply(self): @@ -87,8 +109,8 @@ def test_quaternion_multiply(self): Notice that here '*' must be implemented with np.dot, and for rotation matrix, T.T == T^-1 """ - Tb1 = transform.rotation_matrix(45 * np.pi / 180., (0, 0, 1)) - Tbs = transform.rotation_matrix(90 * np.pi / 180., (0, 1, 0)) + Tb1 = transform.rotation_matrix(45 * np.pi / 180.0, (0, 0, 1)) + Tbs = transform.rotation_matrix(90 * np.pi / 180.0, (0, 1, 0)) T1s = np.dot(np.transpose(Tb1), Tbs) Ts1 = np.transpose(T1s) @@ -110,15 +132,19 @@ def test_2d_rotation(self): def test_most_used_transforms(self): """This test output well-used transformations around x-, y-, and z- axes in quaternion""" # Only rotate around +Z axis - q = transform.quaternion_from_matrix(transform.euler_matrix(0, 0, -np.pi * 0.25)) # -45 - self.assertTrue(common.all_close(q, [0., 0, -0.38268343, 0.92387953])) + q = transform.quaternion_from_matrix( + transform.euler_matrix(0, 0, -np.pi * 0.25) + ) # -45 + self.assertTrue(common.all_close(q, [0.0, 0, -0.38268343, 0.92387953])) # Only rotate around +Y axis - q = transform.quaternion_from_matrix(transform.euler_matrix(0, -np.pi * 0.5, 0)) # -90 - self.assertTrue(common.all_close(q, [0., -0.70710678, 0., 0.70710678])) + q = transform.quaternion_from_matrix( + transform.euler_matrix(0, -np.pi * 0.5, 0) + ) # -90 + self.assertTrue(common.all_close(q, [0.0, -0.70710678, 0.0, 0.70710678])) # Only rotate around +X axis q = transform.quaternion_from_matrix(transform.euler_matrix(np.pi, 0, 0)) # 180 - self.assertTrue(common.all_close(q, [1., 0., 0., 0.])) + self.assertTrue(common.all_close(q, [1.0, 0.0, 0.0, 0.0])) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/src/rotools/tests/test_web_client.py b/src/rotools/tests/test_web_client.py index a9cfcf1..373e838 100644 --- a/src/rotools/tests/test_web_client.py +++ b/src/rotools/tests/test_web_client.py @@ -9,18 +9,17 @@ class Test(unittest.TestCase): - def test_server_not_found(self): """0.0.0.0 is a valid ip indicating the local host, whereas 9092 is not a valid port under the default config (9090) """ configs = { - 'ip': '0.0.0.0', - 'port': 9092, + "ip": "0.0.0.0", + "port": 9092, } with self.assertRaises(ConnectionRefusedError): cli.WebsocketClient(configs) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/src/rotools/tests/unfin/test_policy_dmp.py b/src/rotools/tests/unfin/test_policy_dmp.py index f4398be..833c704 100644 --- a/src/rotools/tests/unfin/test_policy_dmp.py +++ b/src/rotools/tests/unfin/test_policy_dmp.py @@ -13,11 +13,10 @@ class Test(unittest.TestCase): - def test_dmp(self): root_dir = os.path.dirname(os.path.realpath(__file__)) print(root_dir) - test_file = os.path.join(root_dir, 'test_data/0-4.npy') + test_file = os.path.join(root_dir, "test_data/0-4.npy") # data is a 3d array, where the first dimension is the demonstrations, # the second dimension is the way points, and the last is the features. @@ -34,32 +33,30 @@ def test_dmp(self): dt = 1.0 times = np.arange(0, dt * len(points), dt).tolist() - demo = {'points': points, 'times': times} + demo = {"points": points, "times": times} dmp_interface = DMPInterface(3, 1000) dmp_interface.learning(demo) dmp_interface.set_active_dmp() positions = dmp_interface.get_dmp_plan( - x_0=[0., 0.16, 0], - x_dot_0=[0, 0, 0], - goal=[0, 0, 0] + x_0=[0.0, 0.16, 0], x_dot_0=[0, 0, 0], goal=[0, 0, 0] ) - save_path = os.path.join(root_dir, 'test_data/out.npy') + save_path = os.path.join(root_dir, "test_data/out.npy") np.save(save_path, positions) - ax = plt.axes(projection='3d') - ax.set_proj_type('ortho') + ax = plt.axes(projection="3d") + ax.set_proj_type("ortho") def visualize_rollout(rollout, show=True): i = np.arange(0, rollout.shape[0]) # way point id x = rollout[:, 0] y = rollout[:, 1] z = rollout[:, 2] - ax.plot3D(x, y, z, 'y') - ax.scatter3D(x, y, z, s=9, c=i, cmap='rainbow') - ax.set_xlabel('x') - ax.set_ylabel('y') - ax.set_zlabel('z') + ax.plot3D(x, y, z, "y") + ax.scatter3D(x, y, z, s=9, c=i, cmap="rainbow") + ax.set_xlabel("x") + ax.set_ylabel("y") + ax.set_zlabel("z") if show: plt.show() @@ -67,5 +64,5 @@ def visualize_rollout(rollout, show=True): visualize_rollout(positions) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/src/rotools/tests/unfin/test_utility_mesh.py b/src/rotools/tests/unfin/test_utility_mesh.py index 9cff290..1920f90 100644 --- a/src/rotools/tests/unfin/test_utility_mesh.py +++ b/src/rotools/tests/unfin/test_utility_mesh.py @@ -9,14 +9,14 @@ class Test(unittest.TestCase): - def test_webots_converter(self): - """ - """ + """ """ converter = mesh.WebotsConverter() - converter.read_proto('/home/dzp/walker_blender/test1.proto', '/home/dzp/walker_blender/test1.idx') - converter.write_to_stl('/home/dzp/walker_blender/out1.stl') + converter.read_proto( + "/home/dzp/walker_blender/test1.proto", "/home/dzp/walker_blender/test1.idx" + ) + converter.write_to_stl("/home/dzp/walker_blender/out1.stl") -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/src/rotools/tests/unfin/test_webots_interface.py b/src/rotools/tests/unfin/test_webots_interface.py index 7131d06..ee5f79c 100644 --- a/src/rotools/tests/unfin/test_webots_interface.py +++ b/src/rotools/tests/unfin/test_webots_interface.py @@ -11,7 +11,6 @@ class Test(unittest.TestCase): - def test_ur_interface(self): """Before carrying this test, make sure the controller of the robot is chosen as , and the Webots simulation scene is running with a @@ -21,7 +20,7 @@ def test_ur_interface(self): np.set_printoptions(precision=4) ur_interface = webots_interface.WebotsInterfaceUR() js = ur_interface.get_joint_state() - print('ur init joint state \n', js) + print("ur init joint state \n", js) pi_2 = np.pi / 2 q = [0, -0.8, 0.8, 0, pi_2, 0] @@ -29,15 +28,15 @@ def test_ur_interface(self): ur_interface.go_to_joint_state(q) js = ur_interface.get_joint_state() - print('ur current joint state \n', js) + print("ur current joint state \n", js) # Transformation matrix from base frame to TCP frame T = ur_interface.get_tcp_pose() - print('ur tcp pose \n', T) + print("ur tcp pose \n", T) t = T[:3, 3] q = transform.quaternion_from_matrix(T) - print('ur tcp in translation & quaternion \n', t, q) + print("ur tcp in translation & quaternion \n", t, q) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/src/rotools/tests/unfin/test_xsens.py b/src/rotools/tests/unfin/test_xsens.py index 62f05e4..d2608a4 100644 --- a/src/rotools/tests/unfin/test_xsens.py +++ b/src/rotools/tests/unfin/test_xsens.py @@ -8,16 +8,15 @@ class Test(unittest.TestCase): - def test_receiver(self): UDP_IP = "127.0.0.1" UDP_PORT = 9763 - MESSAGE = b'MXTP02\x00\x02Vp\x80?\x02"@\xff\x01\x17\x00(\x00\x00\x07\xe0\x00\x00\x00\x01\xbeH\x8b\xdd\xbfFir?\x14\xad"?}N\xc8\xbcM\x82\x11=\xc9\x08\xb2\xbd\xd8\x0f1\x00\x00\x00\x02\xbe>\x9a\xcb\xbfE\xa7T?06\xe6?|\xffd\xbb\xec\xf7g>\x0b\xa0\xe8\xbd\x8c\x01"\x00\x00\x00\x03\xbe&7\xc4\xbfE\xc0l?E\xe9\x95?~K0\xbb\xf5f\xb7=\xd2\xa9\x1c\xbdR\xd5\x8e\x00\x00\x00\x04\xbe\x16b^\xbfE\xa2d?Z&\x15?~\x00\n\xbb\x8c7e=\xf5\x10\x8b\xbd\x0fh\r\x00\x00\x00\x05\xbe\x03\xe0\xaa\xbfE\x9c\xc5?n80?{\xa23\xba:\xee\xa4>:\xe0i\xbc\xba\x19\xdc\x00\x00\x00\x06\xbd\xb3J\xa3\xbfE\xd0\xac?\x84\xd7\xb2?t\xf1\xf0;\x038L>\x94\xd4t\xba\x85t\x82\x00\x00\x00\x07\xbd\x02?\xac\xbfE\xed\x94?\x8fj\x1e?{S\x8b;\xdb$f>>\x95\xd4=\x1e]\xb6\x00\x00\x00\x08\xbd\xda\x9e?\xbfMG\xb6?}\xd7\xf5?vm\x07<\x0f\xfaI>\x86\xb3A\xbd\x83AY\x00\x00\x00\t\xbe\x01\x9f\xbe\xbft\xa0\x7f?~\x82|?b\xb1q>\xd2;\xf5\xbe]\xb9\xf2\xbc\x9c\xa3\xd0\x00\x00\x00\n\xbd\xb6\x9d\x83\xbf\x90WC?M\x91\x8e?{~\xfa=\xbf\x9d\x8d\xbd\xef\xc0v=\xe4<\x16\x00\x00\x00\x0b\xbd\t\xcf1\xbf\xac\x8f\x14?DF\xe1?\'q\xc2\xbd\x83\xbfz\xbf@(j=\x8b%y\x00\x00\x00\x0c\xbd\xd5?>\xbf>,\x9b?}\xb2o?v|\xc3\xbd\xa9\xa3\xea>\x83|Y\xbc7Z@\x00\x00\x00\r\xbd\xdb\xe7?\xbf\x17\x0c]?w#\x95?+Xp\xbf"?\x8c\xbe\\\xe4y\xbe\xa4\xf4\xd9\x00\x00\x00\x0e=\x9b=\xc8\xbf\x17\xc7\x11?G\xef\x11\xbe\x06\xc7\xd7>\xe1\xd2\xec>)\xb8\x9d\xbfL\xafn?S\xefI?z\xde\x92\xbdKH\xa6\xbe@h\x8b=3\x1a\xa0\x00\x00\x00\x10\xbe[\xbd~\xbf]\x97\x0c?\x16qc?D\x16\xb0\xbe\x85Z1\xbf\r\x18\x9b\xbeQ\re\x00\x00\x00\x11>F7\xf0\xbf\x9a\xf8B>\xe49w\xbfoh\x17=>\x06\xda\xbe\x03(\xbf>\xa7b\x03\x00\x00\x00\x12=\xc8\xb4(\xbf\x9b&J=\xd2\xed\x13?~L\x17\xbc1WY<\xa2\xac\x83\xbd\xe79\xd4\x00\x00\x00\x13>\x885V\xbf\xa0q\x99<\x94N\x85?~\\\xb78\xe4\xbc\x1c:{\x8e\xbe\xbd\xe7H\xf2\x00\x00\x00\x14\xbe4\xd9\xd1\xbf/2\x8d?\x14E\xa7\xbf@ed>*g\x9e?\x1e\x97\xaa>\x1d\xa6=\x00\x00\x00\x15>\x9e\xbc\x9e\xbfm\x9d\xb9>\xf6u\xc8?y\x08\xe9\xbd\x0b \n\xbdF\x06\xb9\xbeee\x16\x00\x00\x00\x16>\xad?\xb5\xbfu\xa6\x9b>\x03\x12\x16\xbf{\x9e\xf2=\x94\xfb\xe8>\x08\xe4\xe6=\xd4d\xf4\x00\x00\x00\x17?\x06E1\xbf\x80\xb8b=\xd0\x9d\xff\xbf{\x9f\x1f=\x95\x02!>\x08\xdd\xf9=\xd4e\x1a\x00\x00\x00\x18>)\xb8\x9d\xbfL\xafn?S\xefI?z\xde\x92\xbdKH\xa6\xbe@h\x8b=3\x1a\xa0\x00\x00\x00\x19>B\xbch\xbfD\x89^?U\xbe$?T\x95\x13>\'\xb0\x17?\x08Hq\xbcq\x9f\xcb\x00\x00\x00\x1a>Jf\xb2\xbf;k\xad?X6_?T\x95\x13>\'\xb0\x17?\x08Hq\xbcq\x9f\xcb\x00\x00\x00\x1b>Q!\x16\xbf3kK?ZaO?T\x95\x13>\'\xb0\x17?\x08Hq\xbcq\x9f\xcb\x00\x00\x00\x1c>3S<\xbfC\x18\xb2?T\x10`?z\xaaf\xbc\xf3|S\xbeD\xaao\xbdqV4\x00\x00\x00\x1d>;9\x1c\xbf3\xa9\x1f?S\x82\x91?|\x10\x8e\xbb\xb9\x84\\\xbe\x1f,\x84=\xa2\xbb\xea\x00\x00\x00\x1e>4A"\xbf(\x8b\x10?S\x1b8?|n\x99\xbc\xc4\xde\xc9\xbe\x140w=\xa0\xbe:\x00\x00\x00\x1f>0\x9e\x9b\xbf"z\xa6?R\xac\xe4?|\x9aD=m\xcc\x91\xbe\x02|\x7f=\xa8[!\x00\x00\x00 >\'\x14\x99\xbfB\xec\xe9?R\xd0\xb1?z\xde\x92\xbdKH\xa6\xbe@h\x8b=3\x1a\xa0\x00\x00\x00!>#\x08+\xbf3\xf6O?Q\x19L?ue\xb5\xbd\x04y%\xbe\x80\xde\x11>\x04k\x14\x00\x00\x00">\x17\x9d\xa6\xbf(\x143?O\x88e?t\x19\x92\xbdB\x85\x89\xbe\x8b\xb2\xab=\xf3@\xd9\x00\x00\x00#>\x11\xc4]\xbf!\x075?Nfr?r\xd9\xba<\xccP,\xbe\x84\xf0\x95>7T\x91\x00\x00\x00$>\x1b\x03P\xbfC\xc8\x1f?Q\xac\x93?xws\xbd\x8b\xe1L\xbe:O\xdc>\x11\x9b\xd3\x00\x00\x00%>\x0cI:\xbf5\xdaY?N\xf8\xe2?w\xf5\x87\xbc\x8b\x1d\x9f\xbe]\xed\xd4=\xf7="\x00\x00\x00&>\x01\xb0\x9f\xbf*\x81J?M\xf9\xb8?w\x95d\xbdG[\x11\xbe[\xad\xee>\x02\xa8.\x00\x00\x00\'=\xf6\xd8>\xbf#\xd3\xf9?L\xf1]?x-\xcb=\x86\xd4\xd5\xbeM\x0f@>\x00q\x81\x00\x00\x00(>\x11\xbc\x83\xbfED\x04?P\xde\x80?r\xcd\xa3\xbd\xb4\xdco\xbe1F%>\x80-R\x00\x00\x00)=\xf1\xfa}\xbf9M\xba?MT]?x;\x1b<\xaa\x85k\xbeB\'\xb3>\x1c\x9d\x86\x00\x00\x00*=\xda1C\xbf0\x01(?M(K?xj\xbe;\xe3\xd8\x99\xbe8\xd6T>$E-\x00\x00\x00+=\xcc\xe07\xbf*\xf9g?L\xeb\xfb?y\xd59;y\x9ey\xbe6c\xe3>\x00\xf6\x04\x00\x00\x00,\xbd\t\xcf1\xbf\xac\x8f\x14?DF\xe1?\'q\xc2\xbd\x83\xbfz\xbf@(j=\x8b%y\x00\x00\x00-\xbd\x1a\x9d!\xbf\xaf\x8b\x17?L\xc3\xd8?u\x15\xca\xbe\x8b]\xbe\xbd\xa4)\xde\xbd]\xf8\xaf\x00\x00\x00.\xbd1W\x8c\xbf\xb3\x9e\x98?Q\xb49?u\x15\xca\xbe\x8b]\xbe\xbd\xa4)\xde\xbd]\xf8\xaf\x00\x00\x00/\xbdEJ\xa1\xbf\xb72\x86?V\n\x06?u\x15\xca\xbe\x8b]\xbe\xbd\xa4)\xde\xbd]\xf8\xaf\x00\x00\x000\xbd\x12\x13\xde\xbf\xb0\xd6\xb0?I2\x1d?$\xb5\xbe\xbe\x11\xd6&\xbf=b\x80>\x0b/a\x00\x00\x001\xbd\x1b\x00\x08\xbf\xb8\x05V?O.T?:\x85\xfa\xbd\xa7|\xdb\xbf,\x0e3=\xd45\xc4\x00\x00\x002\xbd\x13\x9a\x05\xbf\xbdtm?R\x17\\?=\x00]\xbd\x8d\x93\xcd\xbf)O\x15=\xe7G\n\x00\x00\x003\xbd\x0c1\xee\xbf\xc0k\xa6?S\xa3\x0b?\xbd(\xc7c\xbf\xbcUn?>\xaf8?\x13\x80x\xbe>\xe9e\xbfK\xa78\xbc\xa5\x1f\xfc\x00\x00\x00?\xbdC\xec\x0e\xbf\xbe\xcc\x91??\xa7b?\x14\xbd\xa9\xbeT\xfb`\xbfIn\x9f;\xbc\xe8\xd5' + MESSAGE = b"MXTP02\x00\x02Vp\x80?\x02\"@\xff\x01\x17\x00(\x00\x00\x07\xe0\x00\x00\x00\x01\xbeH\x8b\xdd\xbfFir?\x14\xad\"?}N\xc8\xbcM\x82\x11=\xc9\x08\xb2\xbd\xd8\x0f1\x00\x00\x00\x02\xbe>\x9a\xcb\xbfE\xa7T?06\xe6?|\xffd\xbb\xec\xf7g>\x0b\xa0\xe8\xbd\x8c\x01\"\x00\x00\x00\x03\xbe&7\xc4\xbfE\xc0l?E\xe9\x95?~K0\xbb\xf5f\xb7=\xd2\xa9\x1c\xbdR\xd5\x8e\x00\x00\x00\x04\xbe\x16b^\xbfE\xa2d?Z&\x15?~\x00\n\xbb\x8c7e=\xf5\x10\x8b\xbd\x0fh\r\x00\x00\x00\x05\xbe\x03\xe0\xaa\xbfE\x9c\xc5?n80?{\xa23\xba:\xee\xa4>:\xe0i\xbc\xba\x19\xdc\x00\x00\x00\x06\xbd\xb3J\xa3\xbfE\xd0\xac?\x84\xd7\xb2?t\xf1\xf0;\x038L>\x94\xd4t\xba\x85t\x82\x00\x00\x00\x07\xbd\x02?\xac\xbfE\xed\x94?\x8fj\x1e?{S\x8b;\xdb$f>>\x95\xd4=\x1e]\xb6\x00\x00\x00\x08\xbd\xda\x9e?\xbfMG\xb6?}\xd7\xf5?vm\x07<\x0f\xfaI>\x86\xb3A\xbd\x83AY\x00\x00\x00\t\xbe\x01\x9f\xbe\xbft\xa0\x7f?~\x82|?b\xb1q>\xd2;\xf5\xbe]\xb9\xf2\xbc\x9c\xa3\xd0\x00\x00\x00\n\xbd\xb6\x9d\x83\xbf\x90WC?M\x91\x8e?{~\xfa=\xbf\x9d\x8d\xbd\xef\xc0v=\xe4<\x16\x00\x00\x00\x0b\xbd\t\xcf1\xbf\xac\x8f\x14?DF\xe1?'q\xc2\xbd\x83\xbfz\xbf@(j=\x8b%y\x00\x00\x00\x0c\xbd\xd5?>\xbf>,\x9b?}\xb2o?v|\xc3\xbd\xa9\xa3\xea>\x83|Y\xbc7Z@\x00\x00\x00\r\xbd\xdb\xe7?\xbf\x17\x0c]?w#\x95?+Xp\xbf\"?\x8c\xbe\\\xe4y\xbe\xa4\xf4\xd9\x00\x00\x00\x0e=\x9b=\xc8\xbf\x17\xc7\x11?G\xef\x11\xbe\x06\xc7\xd7>\xe1\xd2\xec>)\xb8\x9d\xbfL\xafn?S\xefI?z\xde\x92\xbdKH\xa6\xbe@h\x8b=3\x1a\xa0\x00\x00\x00\x10\xbe[\xbd~\xbf]\x97\x0c?\x16qc?D\x16\xb0\xbe\x85Z1\xbf\r\x18\x9b\xbeQ\re\x00\x00\x00\x11>F7\xf0\xbf\x9a\xf8B>\xe49w\xbfoh\x17=>\x06\xda\xbe\x03(\xbf>\xa7b\x03\x00\x00\x00\x12=\xc8\xb4(\xbf\x9b&J=\xd2\xed\x13?~L\x17\xbc1WY<\xa2\xac\x83\xbd\xe79\xd4\x00\x00\x00\x13>\x885V\xbf\xa0q\x99<\x94N\x85?~\\\xb78\xe4\xbc\x1c:{\x8e\xbe\xbd\xe7H\xf2\x00\x00\x00\x14\xbe4\xd9\xd1\xbf/2\x8d?\x14E\xa7\xbf@ed>*g\x9e?\x1e\x97\xaa>\x1d\xa6=\x00\x00\x00\x15>\x9e\xbc\x9e\xbfm\x9d\xb9>\xf6u\xc8?y\x08\xe9\xbd\x0b \n\xbdF\x06\xb9\xbeee\x16\x00\x00\x00\x16>\xad?\xb5\xbfu\xa6\x9b>\x03\x12\x16\xbf{\x9e\xf2=\x94\xfb\xe8>\x08\xe4\xe6=\xd4d\xf4\x00\x00\x00\x17?\x06E1\xbf\x80\xb8b=\xd0\x9d\xff\xbf{\x9f\x1f=\x95\x02!>\x08\xdd\xf9=\xd4e\x1a\x00\x00\x00\x18>)\xb8\x9d\xbfL\xafn?S\xefI?z\xde\x92\xbdKH\xa6\xbe@h\x8b=3\x1a\xa0\x00\x00\x00\x19>B\xbch\xbfD\x89^?U\xbe$?T\x95\x13>'\xb0\x17?\x08Hq\xbcq\x9f\xcb\x00\x00\x00\x1a>Jf\xb2\xbf;k\xad?X6_?T\x95\x13>'\xb0\x17?\x08Hq\xbcq\x9f\xcb\x00\x00\x00\x1b>Q!\x16\xbf3kK?ZaO?T\x95\x13>'\xb0\x17?\x08Hq\xbcq\x9f\xcb\x00\x00\x00\x1c>3S<\xbfC\x18\xb2?T\x10`?z\xaaf\xbc\xf3|S\xbeD\xaao\xbdqV4\x00\x00\x00\x1d>;9\x1c\xbf3\xa9\x1f?S\x82\x91?|\x10\x8e\xbb\xb9\x84\\\xbe\x1f,\x84=\xa2\xbb\xea\x00\x00\x00\x1e>4A\"\xbf(\x8b\x10?S\x1b8?|n\x99\xbc\xc4\xde\xc9\xbe\x140w=\xa0\xbe:\x00\x00\x00\x1f>0\x9e\x9b\xbf\"z\xa6?R\xac\xe4?|\x9aD=m\xcc\x91\xbe\x02|\x7f=\xa8[!\x00\x00\x00 >'\x14\x99\xbfB\xec\xe9?R\xd0\xb1?z\xde\x92\xbdKH\xa6\xbe@h\x8b=3\x1a\xa0\x00\x00\x00!>#\x08+\xbf3\xf6O?Q\x19L?ue\xb5\xbd\x04y%\xbe\x80\xde\x11>\x04k\x14\x00\x00\x00\">\x17\x9d\xa6\xbf(\x143?O\x88e?t\x19\x92\xbdB\x85\x89\xbe\x8b\xb2\xab=\xf3@\xd9\x00\x00\x00#>\x11\xc4]\xbf!\x075?Nfr?r\xd9\xba<\xccP,\xbe\x84\xf0\x95>7T\x91\x00\x00\x00$>\x1b\x03P\xbfC\xc8\x1f?Q\xac\x93?xws\xbd\x8b\xe1L\xbe:O\xdc>\x11\x9b\xd3\x00\x00\x00%>\x0cI:\xbf5\xdaY?N\xf8\xe2?w\xf5\x87\xbc\x8b\x1d\x9f\xbe]\xed\xd4=\xf7=\"\x00\x00\x00&>\x01\xb0\x9f\xbf*\x81J?M\xf9\xb8?w\x95d\xbdG[\x11\xbe[\xad\xee>\x02\xa8.\x00\x00\x00'=\xf6\xd8>\xbf#\xd3\xf9?L\xf1]?x-\xcb=\x86\xd4\xd5\xbeM\x0f@>\x00q\x81\x00\x00\x00(>\x11\xbc\x83\xbfED\x04?P\xde\x80?r\xcd\xa3\xbd\xb4\xdco\xbe1F%>\x80-R\x00\x00\x00)=\xf1\xfa}\xbf9M\xba?MT]?x;\x1b<\xaa\x85k\xbeB'\xb3>\x1c\x9d\x86\x00\x00\x00*=\xda1C\xbf0\x01(?M(K?xj\xbe;\xe3\xd8\x99\xbe8\xd6T>$E-\x00\x00\x00+=\xcc\xe07\xbf*\xf9g?L\xeb\xfb?y\xd59;y\x9ey\xbe6c\xe3>\x00\xf6\x04\x00\x00\x00,\xbd\t\xcf1\xbf\xac\x8f\x14?DF\xe1?'q\xc2\xbd\x83\xbfz\xbf@(j=\x8b%y\x00\x00\x00-\xbd\x1a\x9d!\xbf\xaf\x8b\x17?L\xc3\xd8?u\x15\xca\xbe\x8b]\xbe\xbd\xa4)\xde\xbd]\xf8\xaf\x00\x00\x00.\xbd1W\x8c\xbf\xb3\x9e\x98?Q\xb49?u\x15\xca\xbe\x8b]\xbe\xbd\xa4)\xde\xbd]\xf8\xaf\x00\x00\x00/\xbdEJ\xa1\xbf\xb72\x86?V\n\x06?u\x15\xca\xbe\x8b]\xbe\xbd\xa4)\xde\xbd]\xf8\xaf\x00\x00\x000\xbd\x12\x13\xde\xbf\xb0\xd6\xb0?I2\x1d?$\xb5\xbe\xbe\x11\xd6&\xbf=b\x80>\x0b/a\x00\x00\x001\xbd\x1b\x00\x08\xbf\xb8\x05V?O.T?:\x85\xfa\xbd\xa7|\xdb\xbf,\x0e3=\xd45\xc4\x00\x00\x002\xbd\x13\x9a\x05\xbf\xbdtm?R\x17\\?=\x00]\xbd\x8d\x93\xcd\xbf)O\x15=\xe7G\n\x00\x00\x003\xbd\x0c1\xee\xbf\xc0k\xa6?S\xa3\x0b?\xbd(\xc7c\xbf\xbcUn?>\xaf8?\x13\x80x\xbe>\xe9e\xbfK\xa78\xbc\xa5\x1f\xfc\x00\x00\x00?\xbdC\xec\x0e\xbf\xbe\xcc\x91??\xa7b?\x14\xbd\xa9\xbeT\xfb`\xbfIn\x9f;\xbc\xe8\xd5" sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP while True: sock.sendto(MESSAGE, (UDP_IP, UDP_PORT)) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/src/rotools/utility/color_palette.py b/src/rotools/utility/color_palette.py index 163bd00..f347d92 100644 --- a/src/rotools/utility/color_palette.py +++ b/src/rotools/utility/color_palette.py @@ -4,5 +4,5 @@ from matplotlib.colors import ListedColormap, LinearSegmentedColormap # https://matplotlib.org/stable/tutorials/colors/colormaps.html -bwr_color_palette = cm.get_cmap('bwr', 7) +bwr_color_palette = cm.get_cmap("bwr", 7) # print(bwr_color_palette(1.)) diff --git a/src/rotools/utility/common.py b/src/rotools/utility/common.py index 56297f3..660e620 100644 --- a/src/rotools/utility/common.py +++ b/src/rotools/utility/common.py @@ -12,7 +12,7 @@ import moveit_msgs.msg as moveit_msg import trajectory_msgs.msg as traj_msg except ImportError: - print('Failed to load rospy, maybe in a virtual env without sourcing setup.bash') + print("Failed to load rospy, maybe in a virtual env without sourcing setup.bash") rospy = None geo_msg = None moveit_msg = None @@ -23,30 +23,38 @@ def print_debug(content): """Print information with green.""" - print(''.join(['\033[1m\033[92m', content, '\033[0m'])) + print("".join(["\033[1m\033[92m", content, "\033[0m"])) def print_info(content): """Print information with sky blue.""" - print(''.join(['\033[1m\033[94m', content, '\033[0m'])) + print("".join(["\033[1m\033[94m", content, "\033[0m"])) def print_warn(content): """Print warning with yellow.""" - print(''.join(['\033[1m\033[93m[Deprecated, use print_warning instead] ', content, '\033[0m'])) + print( + "".join( + [ + "\033[1m\033[93m[Deprecated, use print_warning instead] ", + content, + "\033[0m", + ] + ) + ) def print_warning(content): """Print a warning with yellow.""" - print(''.join(['\033[1m\033[93m', content, '\033[0m'])) + print("".join(["\033[1m\033[93m", content, "\033[0m"])) def print_error(content): """Print error with red.""" - print(''.join(['\033[1m\033[91m', content, '\033[0m'])) + print("".join(["\033[1m\033[91m", content, "\033[0m"])) -def all_close(values_a, values_b, tolerance=1.e-8): +def all_close(values_a, values_b, tolerance=1.0e-8): """Test if a series of values are all within a tolerance of their counterparts in another series. Args: @@ -61,7 +69,7 @@ def all_close(values_a, values_b, tolerance=1.e-8): NotImplementedError if the input type is not supported. """ if tolerance <= 0: - raise ValueError('Given tolerance {} should be larger than 0'.format(tolerance)) + raise ValueError("Given tolerance {} should be larger than 0".format(tolerance)) return np.allclose(to_list(values_a), to_list(values_b), atol=tolerance) @@ -77,8 +85,15 @@ def to_list(values): if isinstance(values, geo_msg.PoseStamped): output = to_list(values.pose) elif isinstance(values, geo_msg.Pose): - output = [values.position.x, values.position.y, values.position.z, values.orientation.x, values.orientation.y, - values.orientation.z, values.orientation.w] + output = [ + values.position.x, + values.position.y, + values.position.z, + values.orientation.x, + values.orientation.y, + values.orientation.z, + values.orientation.w, + ] elif isinstance(values, geo_msg.Quaternion): output = [values.x, values.y, values.z, values.w] elif isinstance(values, list) or isinstance(values, tuple): @@ -86,7 +101,9 @@ def to_list(values): elif isinstance(values, np.ndarray): output = values.tolist() else: - raise NotImplementedError('Type {} cannot be converted to list'.format(type(values))) + raise NotImplementedError( + "Type {} cannot be converted to list".format(type(values)) + ) return output @@ -115,7 +132,7 @@ def offset_ros_pose(pose, offset): output.header = pose.header output.pose = offset_ros_pose(pose.pose, offset) else: - raise NotImplementedError('Type {} is not supported'.format(type(pose))) + raise NotImplementedError("Type {} is not supported".format(type(pose))) return output @@ -234,7 +251,7 @@ def to_ros_pose(pose, w_first=False): raise NotImplementedError -def to_ros_pose_stamped(pose, frame_id='', w_first=False): +def to_ros_pose_stamped(pose, frame_id="", w_first=False): """Convert a pose to PoseStamped. Args: @@ -326,7 +343,9 @@ def sd_orientation(orientation): elif isinstance(orientation, list): return sd_orientation(np.array(orientation)) elif isinstance(orientation, geo_msg.Quaternion): - return sd_orientation(np.array([orientation.x, orientation.y, orientation.z, orientation.w])) + return sd_orientation( + np.array([orientation.x, orientation.y, orientation.z, orientation.w]) + ) else: raise NotImplementedError @@ -349,10 +368,14 @@ def to_orientation_matrix(orientation): return transform.quaternion_matrix(orientation)[:3, :3] else: raise NotImplementedError - elif (isinstance(orientation, list) or isinstance(orientation, tuple)) and len(orientation) == 4: + elif (isinstance(orientation, list) or isinstance(orientation, tuple)) and len( + orientation + ) == 4: return to_orientation_matrix(np.array(orientation)) elif isinstance(orientation, geo_msg.Quaternion): - return to_orientation_matrix(np.array([orientation.x, orientation.y, orientation.z, orientation.w])) + return to_orientation_matrix( + np.array([orientation.x, orientation.y, orientation.z, orientation.w]) + ) else: raise NotImplementedError @@ -376,7 +399,7 @@ def to_ros_orientation(ori, check=False, w_first=False): msg = geo_msg.Quaternion() if ori.shape == (4, 4): if check and not robotics.test_if_SE3(ori): - raise ValueError('The input SE(3) matrix is illegal:\n{}'.format(ori)) + raise ValueError("The input SE(3) matrix is illegal:\n{}".format(ori)) q = transform.quaternion_from_matrix(ori) msg.x = q[0] msg.y = q[1] @@ -385,13 +408,17 @@ def to_ros_orientation(ori, check=False, w_first=False): return msg elif ori.shape == (3, 3): if check and not robotics.test_if_SO3(ori): - raise ValueError('The input SO(3) matrix is illegal:\n{}'.format(ori)) + raise ValueError("The input SO(3) matrix is illegal:\n{}".format(ori)) i = transform.identity_matrix() i[:3, :3] = ori return to_ros_orientation(i) elif ori.size == 4: - if check and not np.allclose(np.linalg.norm(ori), 1.): - raise ValueError('The input norm is not close to 1: {}'.format(ori, np.linalg.norm(ori))) + if check and not np.allclose(np.linalg.norm(ori), 1.0): + raise ValueError( + "The input norm is not close to 1: {}".format( + ori, np.linalg.norm(ori) + ) + ) if w_first: msg.w = ori[0] msg.x = ori[1] @@ -403,7 +430,7 @@ def to_ros_orientation(ori, check=False, w_first=False): msg.z = ori[2] msg.w = ori[3] return msg - elif ori.shape == (9, ): + elif ori.shape == (9,): return to_ros_orientation(np.reshape(ori, (3, 3)), check, w_first) else: raise NotImplementedError @@ -499,7 +526,8 @@ def local_pose_to_global_pose(local_to_target, global_to_local): if type(local_to_target) != type(global_to_local): print_warn( "Local to target pose type {} is not the same as global to local pose {}".format( - type(local_to_target), type(global_to_local)) + type(local_to_target), type(global_to_local) + ) ) sd_local_to_target = sd_pose(local_to_target) sd_global_to_local = sd_pose(global_to_local) @@ -530,7 +558,8 @@ def local_aligned_pose_to_global_pose(local_aligned_to_target, global_to_local): if type(local_aligned_to_target) != type(global_to_local): print_warn( "Local aligned to target pose type {} is not the same as global to local pose {}".format( - type(local_aligned_to_target), type(global_to_local)) + type(local_aligned_to_target), type(global_to_local) + ) ) sd_local_aligned_to_target = sd_pose(local_aligned_to_target) sd_global_to_local_aligned = sd_pose(global_to_local) @@ -539,7 +568,9 @@ def local_aligned_pose_to_global_pose(local_aligned_to_target, global_to_local): if isinstance(local_aligned_to_target, geo_msg.Pose): return to_ros_pose(sd_global_to_target) elif isinstance(local_aligned_to_target, geo_msg.PoseStamped): - return to_ros_pose_stamped(sd_global_to_target, local_aligned_to_target.header.frame_id) + return to_ros_pose_stamped( + sd_global_to_target, local_aligned_to_target.header.frame_id + ) else: raise NotImplementedError @@ -578,14 +609,14 @@ def get_path(param_name, value=None): """ path = get_param(param_name, value) if path is None: - raise FileNotFoundError('Failed to get the path from {}'.format(param_name)) - if path.startswith('/'): + raise FileNotFoundError("Failed to get the path from {}".format(param_name)) + if path.startswith("/"): abs_path = path else: - abs_path = os.path.join(os.path.join(os.path.expanduser('~'), path)) + abs_path = os.path.join(os.path.join(os.path.expanduser("~"), path)) if os.path.exists(abs_path): return abs_path - raise FileNotFoundError('Path {} does not exist'.format(abs_path)) + raise FileNotFoundError("Path {} does not exist".format(abs_path)) def pretty_print_configs(configs): @@ -600,32 +631,40 @@ def pretty_print_configs(configs): max_key_len = 0 max_value_len = 0 for key, value in configs.items(): - key_str = '{}'.format(key) + key_str = "{}".format(key) if len(key_str) > max_key_len: max_key_len = len(key_str) if isinstance(value, list) or isinstance(value, tuple): for i in value: - i_str = '{}'.format(i) + i_str = "{}".format(i) if len(i_str) > max_value_len: max_value_len = len(i_str) else: - value_str = '{}'.format(value) + value_str = "{}".format(value) if len(value_str) > max_value_len: max_value_len = len(value_str) - print_info("\n{}{}{}".format('=' * (max_key_len + 1), " ROPORT CONFIGS ", '=' * (max_value_len - 15))) + print_info( + "\n{}{}{}".format( + "=" * (max_key_len + 1), " ROPORT CONFIGS ", "=" * (max_value_len - 15) + ) + ) for key, value in configs.items(): - key_msg = '{message: <{width}}'.format(message=key, width=max_key_len) - empty_key_msg = '{message: <{width}}'.format(message='', width=max_key_len) + key_msg = "{message: <{width}}".format(message=key, width=max_key_len) + empty_key_msg = "{message: <{width}}".format(message="", width=max_key_len) if isinstance(value, list) or isinstance(value, tuple): for i, i_v in enumerate(value): if i == 0: - print_info('{}: {}'.format(key_msg, i_v)) + print_info("{}: {}".format(key_msg, i_v)) else: - print_info('{}: {}'.format(empty_key_msg, i_v)) + print_info("{}: {}".format(empty_key_msg, i_v)) else: - print_info('{}: {}'.format(key_msg, value)) - print_info("{}{}{}\n".format('=' * (max_key_len + 1), " END OF CONFIGS ", '=' * (max_value_len - 15))) + print_info("{}: {}".format(key_msg, value)) + print_info( + "{}{}{}\n".format( + "=" * (max_key_len + 1), " END OF CONFIGS ", "=" * (max_value_len - 15) + ) + ) def encode_image_to_b64(image): @@ -635,10 +674,10 @@ def encode_image_to_b64(image): be 8UC3 (bgr) or 16UC1 (depth) :return: b64_image_str """ - _, img_buffer = cv2.imencode('.png', image) + _, img_buffer = cv2.imencode(".png", image) encoded_buffer = base64.b64encode(img_buffer) # cf. https://code-examples.net/en/q/238024b - return encoded_buffer.decode('utf-8') + return encoded_buffer.decode("utf-8") def decode_b64_to_image(b64_str, is_bgr=True): @@ -658,9 +697,13 @@ def decode_b64_to_image(b64_str, is_bgr=True): img = base64.b64decode(b64_str) # imdecode use the same flag as imread. cf. https://docs.opencv.org/3.4/d8/d6a/group__imgcodecs__flags.html if is_bgr: - return True, cv2.imdecode(np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_COLOR) + return True, cv2.imdecode( + np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_COLOR + ) else: - return True, cv2.imdecode(np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_ANYDEPTH) + return True, cv2.imdecode( + np.frombuffer(img, dtype=np.uint8), cv2.IMREAD_ANYDEPTH + ) except cv2.error: return False, None @@ -690,29 +733,29 @@ def post_http_requests(url, payload, headers=None, params=None): def byte_to_str(data, n): - fmt = '!{}c'.format(n) + fmt = "!{}c".format(n) char_array = struct.unpack(fmt, data) - str_out = '' + str_out = "" for c in char_array: - s = c.decode('utf-8') + s = c.decode("utf-8") str_out += s return str_out def byte_to_float(data): - return struct.unpack('!f', data)[0] + return struct.unpack("!f", data)[0] def byte_to_uint32(data): - return struct.unpack('!I', data)[0] + return struct.unpack("!I", data)[0] def byte_to_uint16(data): - return struct.unpack('!H', data)[0] + return struct.unpack("!H", data)[0] def byte_to_uint8(data): - return struct.unpack('!B', data)[0] + return struct.unpack("!B", data)[0] def is_ip_valid(ip): @@ -727,10 +770,10 @@ def is_ip_valid(ip): if not isinstance(ip, str): print_error("IP is not a string") return False - if len(ip.split('.')) != 4: + if len(ip.split(".")) != 4: print_error("IP {} is illegal".format(ip)) return False - if ':' in ip: + if ":" in ip: print_error("IP {} should not contain port number".format(ip)) return False return True @@ -765,14 +808,17 @@ def play_hint_sound(enable): # TODO check playsound could work on Python 3 from playsound import playsound import os.path as osp - misc_dir = osp.dirname(osp.dirname(osp.dirname(osp.dirname(osp.realpath(__file__))))) + + misc_dir = osp.dirname( + osp.dirname(osp.dirname(osp.dirname(osp.realpath(__file__)))) + ) if enable: - misc_path = osp.join(misc_dir, 'misc/audio/Sophia_function_activated.mp3') + misc_path = osp.join(misc_dir, "misc/audio/Sophia_function_activated.mp3") else: - misc_path = osp.join(misc_dir, 'misc/audio/Sophia_function_deactivated.mp3') + misc_path = osp.join(misc_dir, "misc/audio/Sophia_function_deactivated.mp3") playsound(misc_path) # Not support block=True on Ubuntu except ImportError as e: - rospy.logdebug('Sound not played due to missing dependence: {}'.format(e)) + rospy.logdebug("Sound not played due to missing dependence: {}".format(e)) def wait_for_service(srv, time=0.2): @@ -804,10 +850,14 @@ def create_publishers(namespace, topic_ids, topic_types, queue_size=1): if isinstance(topic_ids, list): publishers = dict() for topic_id, topic_type in zip(topic_ids, topic_types): - publishers[topic_id] = create_publishers(namespace, topic_id, topic_type, queue_size) + publishers[topic_id] = create_publishers( + namespace, topic_id, topic_type, queue_size + ) return publishers else: - return rospy.Publisher(namespace + '/' + topic_ids, topic_types, queue_size=queue_size) + return rospy.Publisher( + namespace + "/" + topic_ids, topic_types, queue_size=queue_size + ) def create_service_proxies(namespace, service_ids, service_types): @@ -827,9 +877,11 @@ def create_service_proxies(namespace, service_ids, service_types): if isinstance(service_ids, list): service_proxies = dict() for service_id, service_type in zip(service_ids, service_types): - service_proxies[service_id] = create_service_proxies(namespace, service_id, service_type) + service_proxies[service_id] = create_service_proxies( + namespace, service_id, service_type + ) return service_proxies else: - service_id = namespace + '/' + service_ids + service_id = namespace + "/" + service_ids wait_for_service(service_id) return rospy.ServiceProxy(service_id, service_types) diff --git a/src/rotools/utility/emergency_stop.py b/src/rotools/utility/emergency_stop.py index b8d3890..ad6bdd1 100644 --- a/src/rotools/utility/emergency_stop.py +++ b/src/rotools/utility/emergency_stop.py @@ -15,17 +15,27 @@ def __init__(self, function_name=None): self._enable = False if Listener is not None: - hot_key = 'Alt' + hot_key = "Alt" if isinstance(function_name, str): - print_warn('{} is deactivated, press {} button to activate/deactivate'.format(function_name, hot_key)) + print_warn( + "{} is deactivated, press {} button to activate/deactivate".format( + function_name, hot_key + ) + ) else: - print_warn('Function is deactivated, press {} button to activate/deactivate'.format(hot_key)) + print_warn( + "Function is deactivated, press {} button to activate/deactivate".format( + hot_key + ) + ) self.listener = Listener(on_press=self._on_press) self.listener.start() # start the thread and run subsequent codes else: - print_warn('{}'.format(err)) - print_warn('To use keyboard interactions: sudo pip install pynput playsound') + print_warn("{}".format(err)) + print_warn( + "To use keyboard interactions: sudo pip install pynput playsound" + ) def _on_press(self, key): if key == Key.alt or key == Key.alt_r: @@ -35,9 +45,9 @@ def _on_press(self, key): def set_status(self, status): self._enable = status if self._enable: - print_warn('\nActivated') + print_warn("\nActivated") else: - print_debug('\nStopped') + print_debug("\nStopped") play_hint_sound(status) @property diff --git a/src/rotools/utility/mesh.py b/src/rotools/utility/mesh.py index b17d267..b3254d9 100644 --- a/src/rotools/utility/mesh.py +++ b/src/rotools/utility/mesh.py @@ -28,7 +28,7 @@ def read_proto(self, vertex_file, index_file): for i in index_list: # index of the vertexes belonging to the same face try: - i1, i2, i3 = i.split(' ') + i1, i2, i3 = i.split(" ") v1 = point_list[int(i1)] v2 = point_list[int(i2)] v3 = point_list[int(i3)] @@ -40,21 +40,23 @@ def read_proto(self, vertex_file, index_file): def read_coordinate_file(coordinate_file): """The coordinates of vertexes are arranged in x y z order. Note that ',' may be present after z. - + :param coordinate_file: text file containing the coordinate values - :return: + :return: """ - with open(coordinate_file, 'r') as v_file: + with open(coordinate_file, "r") as v_file: coord_raw = v_file.readline() coordinates = coord_raw.replace(",", "") - c_list = coordinates.split(' ') + c_list = coordinates.split(" ") # 3 coordinates form a point assert len(c_list) % 3 == 0 sz = len(c_list) point_list = [] for i in range(0, sz, 3): - point_list.append("{} {} {}".format(c_list[i], c_list[i + 1], c_list[i + 2])) - print('Total vertexes #', len(point_list)) + point_list.append( + "{} {} {}".format(c_list[i], c_list[i + 1], c_list[i + 2]) + ) + print("Total vertexes #", len(point_list)) return point_list @staticmethod @@ -65,10 +67,10 @@ def read_index_file(index_file): :param index_file: text file containing the coordIndex :return: index_list """ - with open(index_file, 'r') as i_file: + with open(index_file, "r") as i_file: indexes_raw = i_file.readline() indexes = indexes_raw.replace(",", "") - index_list = indexes.split(' -1 ') + index_list = indexes.split(" -1 ") return index_list def write_to_stl(self, stl_file): @@ -85,14 +87,14 @@ def write_to_stl(self, stl_file): :param stl_file: :return: """ - with open(stl_file, 'w') as s_file: - s_file.write('solid Exported from RoTools\n') + with open(stl_file, "w") as s_file: + s_file.write("solid Exported from RoTools\n") for face in self.face_list: # s_file.write('face normal {} {} {}'.format(nx, ny, nz)) - s_file.write('outer loop\n') - s_file.write('vertex {}\n'.format(face[0])) - s_file.write('vertex {}\n'.format(face[1])) - s_file.write('vertex {}\n'.format(face[2])) - s_file.write('endloop\n') - s_file.write('endfacet\n') - s_file.write('endsolid Exported from RoTools') + s_file.write("outer loop\n") + s_file.write("vertex {}\n".format(face[0])) + s_file.write("vertex {}\n".format(face[1])) + s_file.write("vertex {}\n".format(face[2])) + s_file.write("endloop\n") + s_file.write("endfacet\n") + s_file.write("endsolid Exported from RoTools") diff --git a/src/rotools/utility/message_converter.py b/src/rotools/utility/message_converter.py index cd1b60c..fab18e4 100644 --- a/src/rotools/utility/message_converter.py +++ b/src/rotools/utility/message_converter.py @@ -39,7 +39,7 @@ import copy import collections -python3 = (sys.hexversion > 0x03000000) +python3 = sys.hexversion > 0x03000000 python_list_types = [list, tuple] @@ -53,40 +53,57 @@ python_float_types = [float] ros_to_python_type_map = { - 'bool': [bool], - 'float32': copy.deepcopy(python_float_types + python_int_types), - 'float64': copy.deepcopy(python_float_types + python_int_types), - 'int8': copy.deepcopy(python_int_types), - 'int16': copy.deepcopy(python_int_types), - 'int32': copy.deepcopy(python_int_types), - 'int64': copy.deepcopy(python_int_types), - 'uint8': copy.deepcopy(python_int_types), - 'uint16': copy.deepcopy(python_int_types), - 'uint32': copy.deepcopy(python_int_types), - 'uint64': copy.deepcopy(python_int_types), - 'byte': copy.deepcopy(python_int_types), - 'char': copy.deepcopy(python_int_types), - 'string': copy.deepcopy(python_string_types) + "bool": [bool], + "float32": copy.deepcopy(python_float_types + python_int_types), + "float64": copy.deepcopy(python_float_types + python_int_types), + "int8": copy.deepcopy(python_int_types), + "int16": copy.deepcopy(python_int_types), + "int32": copy.deepcopy(python_int_types), + "int64": copy.deepcopy(python_int_types), + "uint8": copy.deepcopy(python_int_types), + "uint16": copy.deepcopy(python_int_types), + "uint32": copy.deepcopy(python_int_types), + "uint64": copy.deepcopy(python_int_types), + "byte": copy.deepcopy(python_int_types), + "char": copy.deepcopy(python_int_types), + "string": copy.deepcopy(python_string_types), } try: import numpy as np _ros_to_numpy_type_map = { - 'float32': [np.float32, np.int8, np.int16, np.uint8, np.uint16], + "float32": [np.float32, np.int8, np.int16, np.uint8, np.uint16], # don't include int32, because conversion to float may change value: v = np.iinfo(np.int32).max; # np.float32(v) != v - 'float64': [np.float32, np.float64, np.int8, np.int16, np.int32, np.uint8, np.uint16, np.uint32], - 'int8': [np.int8], - 'int16': [np.int8, np.int16, np.uint8], - 'int32': [np.int8, np.int16, np.int32, np.uint8, np.uint16], - 'int64': [np.int8, np.int16, np.int32, np.int64, np.uint8, np.uint16, np.uint32], - 'uint8': [np.uint8], - 'uint16': [np.uint8, np.uint16], - 'uint32': [np.uint8, np.uint16, np.uint32], - 'uint64': [np.uint8, np.uint16, np.uint32, np.uint64], - 'byte': [np.int8], - 'char': [np.uint8], + "float64": [ + np.float32, + np.float64, + np.int8, + np.int16, + np.int32, + np.uint8, + np.uint16, + np.uint32, + ], + "int8": [np.int8], + "int16": [np.int8, np.int16, np.uint8], + "int32": [np.int8, np.int16, np.int32, np.uint8, np.uint16], + "int64": [ + np.int8, + np.int16, + np.int32, + np.int64, + np.uint8, + np.uint16, + np.uint32, + ], + "uint8": [np.uint8], + "uint16": [np.uint8, np.uint16], + "uint32": [np.uint8, np.uint16, np.uint32], + "uint64": [np.uint8, np.uint16, np.uint32, np.uint64], + "byte": [np.int8], + "char": [np.uint8], } # merge type_maps @@ -97,15 +114,35 @@ except ImportError: pass -ros_time_types = ['time', 'duration'] -ros_primitive_types = ['bool', 'byte', 'char', 'int8', 'uint8', 'int16', - 'uint16', 'int32', 'uint32', 'int64', 'uint64', - 'float32', 'float64', 'string'] -ros_header_types = ['Header', 'std_msgs/Header', 'roslib/Header'] - - -def convert_dictionary_to_ros_message(message_type, dictionary, kind='message', strict_mode=True, - check_missing_fields=False, check_types=True, log_level='error'): +ros_time_types = ["time", "duration"] +ros_primitive_types = [ + "bool", + "byte", + "char", + "int8", + "uint8", + "int16", + "uint16", + "int32", + "uint32", + "int64", + "uint64", + "float32", + "float64", + "string", +] +ros_header_types = ["Header", "std_msgs/Header", "roslib/Header"] + + +def convert_dictionary_to_ros_message( + message_type, + dictionary, + kind="message", + strict_mode=True, + check_missing_fields=False, + check_types=True, + log_level="error", +): """ Takes in the message type and a Python dictionary and returns a ROS message. @@ -121,13 +158,13 @@ def convert_dictionary_to_ros_message(message_type, dictionary, kind='message', >>> convert_dictionary_to_ros_message(msg_type, dict_msg, kind) data: True """ - if kind == 'message': + if kind == "message": message_class = roslib.message.get_message_class(message_type) message = message_class() - elif kind == 'request': + elif kind == "request": service_class = roslib.message.get_service_class(message_type) message = service_class._request_class() - elif kind == 'response': + elif kind == "response": service_class = roslib.message.get_service_class(message_type) message = service_class._response_class() else: @@ -142,23 +179,29 @@ def convert_dictionary_to_ros_message(message_type, dictionary, kind='message', if field_name in message_fields: field_type = message_fields[field_name] if field_value is not None: - field_value = _convert_to_ros_type(field_name, field_type, field_value, strict_mode, - check_missing_fields, - check_types) + field_value = _convert_to_ros_type( + field_name, + field_type, + field_value, + strict_mode, + check_missing_fields, + check_types, + ) setattr(message, field_name, field_value) del remaining_message_fields[field_name] else: - error_message = 'ROS message type "{0}" has no field named "{1}"' \ - .format(message_type, field_name) + error_message = 'ROS message type "{0}" has no field named "{1}"'.format( + message_type, field_name + ) if strict_mode: raise ValueError(error_message) else: if log_level not in ["debug", "info", "warning", "error", "critical"]: log_level = "error" - logger = logging.getLogger('rosout') + logger = logging.getLogger("rosout") log_func = getattr(logger, log_level) - log_func('{}! It will be ignored.'.format(error_message)) + log_func("{}! It will be ignored.".format(error_message)) if check_missing_fields and remaining_message_fields: error_message = 'Missing fields "{0}"'.format(remaining_message_fields) @@ -167,8 +210,14 @@ def convert_dictionary_to_ros_message(message_type, dictionary, kind='message', return message -def _convert_to_ros_type(field_name, field_type, field_value, strict_mode=True, check_missing_fields=False, - check_types=True): +def _convert_to_ros_type( + field_name, + field_type, + field_value, + strict_mode=True, + check_missing_fields=False, + check_types=True, +): if _is_ros_binary_type(field_type): field_value = _convert_to_ros_binary(field_type, field_value) elif field_type in ros_time_types: @@ -179,19 +228,31 @@ def _convert_to_ros_type(field_name, field_type, field_value, strict_mode=True, # 2. it doesn't check floats (see ros/genpy#130) # 3. it rejects numpy types, although they can be serialized if check_types and type(field_value) not in ros_to_python_type_map[field_type]: - raise TypeError("Field '{0}' has wrong type {1} (valid types: {2})".format(field_name, type(field_value), - ros_to_python_type_map[ - field_type])) + raise TypeError( + "Field '{0}' has wrong type {1} (valid types: {2})".format( + field_name, type(field_value), ros_to_python_type_map[field_type] + ) + ) field_value = _convert_to_ros_primitive(field_type, field_value) elif _is_field_type_a_primitive_array(field_type): field_value = field_value elif _is_field_type_an_array(field_type): - field_value = _convert_to_ros_array(field_name, field_type, field_value, strict_mode, check_missing_fields, - check_types) + field_value = _convert_to_ros_array( + field_name, + field_type, + field_value, + strict_mode, + check_missing_fields, + check_types, + ) else: - field_value = convert_dictionary_to_ros_message(field_type, field_value, strict_mode=strict_mode, - check_missing_fields=check_missing_fields, - check_types=check_types) + field_value = convert_dictionary_to_ros_message( + field_type, + field_value, + strict_mode=strict_mode, + check_missing_fields=check_missing_fields, + check_types=check_types, + ) return field_value @@ -214,17 +275,17 @@ def _convert_to_ros_binary(field_type, field_value): def _convert_to_ros_time(field_type, field_value): time = None - if field_type == 'time' and field_value == 'now': + if field_type == "time" and field_value == "now": time = rospy.get_rostime() else: - if field_type == 'time': + if field_type == "time": time = rospy.rostime.Time() - elif field_type == 'duration': + elif field_type == "duration": time = rospy.rostime.Duration() - if 'secs' in field_value and field_value['secs'] is not None: - setattr(time, 'secs', field_value['secs']) - if 'nsecs' in field_value and field_value['nsecs'] is not None: - setattr(time, 'nsecs', field_value['nsecs']) + if "secs" in field_value and field_value["secs"] is not None: + setattr(time, "secs", field_value["secs"]) + if "nsecs" in field_value and field_value["nsecs"] is not None: + setattr(time, "nsecs", field_value["nsecs"]) return time @@ -232,16 +293,26 @@ def _convert_to_ros_time(field_type, field_value): def _convert_to_ros_primitive(field_type, field_value): # std_msgs/msg/_String.py always calls encode() on python3, so don't do it here if field_type == "string" and not python3: - field_value = field_value.encode('utf-8') + field_value = field_value.encode("utf-8") return field_value -def _convert_to_ros_array(field_name, field_type, list_value, strict_mode=True, check_missing_fields=False, - check_types=True): +def _convert_to_ros_array( + field_name, + field_type, + list_value, + strict_mode=True, + check_missing_fields=False, + check_types=True, +): # use index to raise ValueError if '[' not present - list_type = field_type[:field_type.index('[')] - return [_convert_to_ros_type(field_name, list_type, value, strict_mode, check_missing_fields, check_types) for value - in list_value] + list_type = field_type[: field_type.index("[")] + return [ + _convert_to_ros_type( + field_name, list_type, value, strict_mode, check_missing_fields, check_types + ) + for value in list_value + ] def convert_ros_message_to_dictionary(message, binary_array_as_bytes=True): @@ -258,7 +329,9 @@ def convert_ros_message_to_dictionary(message, binary_array_as_bytes=True): message_fields = _get_message_fields(message) for field_name, field_type in message_fields: field_value = getattr(message, field_name) - dictionary[field_name] = _convert_from_ros_type(field_type, field_value, binary_array_as_bytes) + dictionary[field_name] = _convert_from_ros_type( + field_type, field_value, binary_array_as_bytes + ) return dictionary @@ -278,15 +351,19 @@ def _convert_from_ros_type(field_type, field_value, binary_array_as_bytes=True): elif _is_field_type_a_primitive_array(field_type): field_value = list(field_value) elif _is_field_type_an_array(field_type): - field_value = _convert_from_ros_array(field_type, field_value, binary_array_as_bytes) + field_value = _convert_from_ros_array( + field_type, field_value, binary_array_as_bytes + ) else: - field_value = convert_ros_message_to_dictionary(field_value, binary_array_as_bytes) + field_value = convert_ros_message_to_dictionary( + field_value, binary_array_as_bytes + ) return field_value def _is_ros_binary_type(field_type): - """ Checks if the field is a binary array one, fixed size or not + """Checks if the field is a binary array one, fixed size or not >>> _is_ros_binary_type("uint8") False @@ -301,33 +378,33 @@ def _is_ros_binary_type(field_type): >>> _is_ros_binary_type("char[3]") True """ - return field_type.startswith('uint8[') or field_type.startswith('char[') + return field_type.startswith("uint8[") or field_type.startswith("char[") def _convert_from_ros_binary(field_type, field_value): - field_value = base64.b64encode(field_value).decode('utf-8') + field_value = base64.b64encode(field_value).decode("utf-8") return field_value def _convert_from_ros_time(field_type, field_value): - field_value = { - 'secs': field_value.secs, - 'nsecs': field_value.nsecs - } + field_value = {"secs": field_value.secs, "nsecs": field_value.nsecs} return field_value def _convert_from_ros_primitive(field_type, field_value): # std_msgs/msg/_String.py always calls decode() on python3, so don't do it here if field_type == "string" and not python3: - field_value = field_value.decode('utf-8') + field_value = field_value.decode("utf-8") return field_value def _convert_from_ros_array(field_type, field_value, binary_array_as_bytes=True): # use index to raise ValueError if '[' not present - list_type = field_type[:field_type.index('[')] - return [_convert_from_ros_type(list_type, value, binary_array_as_bytes) for value in field_value] + list_type = field_type[: field_type.index("[")] + return [ + _convert_from_ros_type(list_type, value, binary_array_as_bytes) + for value in field_value + ] def _get_message_fields(message): @@ -335,11 +412,11 @@ def _get_message_fields(message): def _is_field_type_an_array(field_type): - return field_type.find('[') >= 0 + return field_type.find("[") >= 0 def _is_field_type_a_primitive_array(field_type): - bracket_index = field_type.find('[') + bracket_index = field_type.find("[") if bracket_index < 0: return False else: diff --git a/src/rotools/utility/mjcf.py b/src/rotools/utility/mjcf.py index 5a07b01..ef05637 100644 --- a/src/rotools/utility/mjcf.py +++ b/src/rotools/utility/mjcf.py @@ -123,7 +123,8 @@ } TEXTURES = { - texture_name: os.path.join("textures", texture_file) for (texture_name, texture_file) in TEXTURE_FILES.items() + texture_name: os.path.join("textures", texture_file) + for (texture_name, texture_file) in TEXTURE_FILES.items() } ALL_TEXTURES = TEXTURES.keys() @@ -360,18 +361,29 @@ def get_size(size, size_max, size_min, default_max, default_min): """ if len(default_max) != len(default_min): raise ValueError( - "default_max = {} and default_min = {}".format(str(default_max), str(default_min)) + "default_max = {} and default_min = {}".format( + str(default_max), str(default_min) + ) + " have different lengths" ) if size is not None: if (size_max is not None) or (size_min is not None): - raise ValueError("size = {} overrides size_max = {}, size_min = {}".format(size, size_max, size_min)) + raise ValueError( + "size = {} overrides size_max = {}, size_min = {}".format( + size, size_max, size_min + ) + ) else: if size_max is None: size_max = default_max if size_min is None: size_min = default_min - size = np.array([np.random.uniform(size_min[i], size_max[i]) for i in range(len(default_max))]) + size = np.array( + [ + np.random.uniform(size_min[i], size_max[i]) + for i in range(len(default_max)) + ] + ) return np.array(size) @@ -405,11 +417,11 @@ def add_to_dict(dic, fill_in_defaults=True, default_value=None, **kwargs): def add_prefix( - root, - prefix, - tags="default", - attribs="default", - exclude=None, + root, + prefix, + tags="default", + attribs="default", + exclude=None, ): """ Find all element(s) matching the requested @tag, and appends @prefix to all @attributes if they exist. @@ -431,12 +443,18 @@ def add_prefix( attribs = {attribs} if type(attribs) is str else set(attribs) # Check the current element for matching conditions - if (tags == "default" or root.tag in tags) and (exclude is None or not exclude(root)): + if (tags == "default" or root.tag in tags) and ( + exclude is None or not exclude(root) + ): for attrib in attribs: v = root.get(attrib, None) # Only add prefix if the attribute exist, the current attribute doesn't already begin with prefix, # and the @exclude filter is either None or returns False - if v is not None and not v.startswith(prefix) and (exclude is None or not exclude(v)): + if ( + v is not None + and not v.startswith(prefix) + and (exclude is None or not exclude(v)) + ): root.set(attrib, prefix + v) # Continue recursively searching through the element tree for r in root: @@ -456,7 +474,11 @@ def recolor_collision_geoms(root, rgba, exclude=None): return True if we should exclude the given element / attribute from having its collision geom impacted. """ # Check this body - if root.tag == "geom" and root.get("group") in {None, "0"} and (exclude is None or not exclude(root)): + if ( + root.tag == "geom" + and root.get("group") in {None, "0"} + and (exclude is None or not exclude(root)) + ): root.set("rgba", array_to_string(rgba)) root.attrib.pop("material", None) @@ -548,7 +570,10 @@ def sort_elements(root, parent=None, element_filter=None, _elements_dict=None): # Loop through all possible subtrees for this XML recursively for r in root: _elements_dict = sort_elements( - root=r, parent=root, element_filter=element_filter, _elements_dict=_elements_dict + root=r, + parent=root, + element_filter=element_filter, + _elements_dict=_elements_dict, ) return _elements_dict @@ -616,14 +641,20 @@ def find_elements(root, tags, attribs=None, return_first=True): # Continue recursively searching through the element tree for r in root: if return_first: - elements = find_elements(tags=tags, attribs=attribs, root=r, return_first=return_first) + elements = find_elements( + tags=tags, attribs=attribs, root=r, return_first=return_first + ) if elements is not None: return elements else: - found_elements = find_elements(tags=tags, attribs=attribs, root=r, return_first=return_first) + found_elements = find_elements( + tags=tags, attribs=attribs, root=r, return_first=return_first + ) pre_elements = deepcopy(elements) if found_elements: - elements += found_elements if type(found_elements) is list else [found_elements] + elements += ( + found_elements if type(found_elements) is list else [found_elements] + ) return elements if elements else None @@ -674,9 +705,14 @@ def get_ids(sim, elements, element_type="geom", inplace=False): elif isinstance(elements, dict): # Iterate over each element in dict and recursively repeat for name, ele in elements: - elements[name] = get_ids(sim=sim, elements=ele, element_type=element_type, inplace=True) + elements[name] = get_ids( + sim=sim, elements=ele, element_type=element_type, inplace=True + ) else: # We assume this is an iterable array assert isinstance(elements, Iterable), "Elements must be iterable for get_id!" - elements = [get_ids(sim=sim, elements=ele, element_type=element_type, inplace=True) for ele in elements] + elements = [ + get_ids(sim=sim, elements=ele, element_type=element_type, inplace=True) + for ele in elements + ] return elements diff --git a/src/rotools/utility/robotics.py b/src/rotools/utility/robotics.py index ce0654b..796f83c 100644 --- a/src/rotools/utility/robotics.py +++ b/src/rotools/utility/robotics.py @@ -59,9 +59,9 @@ def vector_to_so3(omega): [ 3, 0, -1], [-2, 1, 0]]) """ - return np.array([[0, -omega[2], omega[1]], - [omega[2], 0, -omega[0]], - [-omega[1], omega[0], 0]]) + return np.array( + [[0, -omega[2], omega[1]], [omega[2], 0, -omega[0]], [-omega[1], omega[0], 0]] + ) def so3_to_vector(so3_mat): @@ -112,7 +112,11 @@ def matrix_exp3(so3_mat): else: theta = axis_angle3(omega_theta)[1] omega_mat = so3_mat / theta - return np.eye(3) + np.sin(theta) * omega_mat + (1 - np.cos(theta)) * np.dot(omega_mat, omega_mat) + return ( + np.eye(3) + + np.sin(theta) * omega_mat + + (1 - np.cos(theta)) * np.dot(omega_mat, omega_mat) + ) def matrix_log3(R): @@ -134,14 +138,17 @@ def matrix_log3(R): return np.zeros((3, 3)) elif acos_input <= -1: if not near_zero(1 + R[2][2]): - omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) \ - * np.array([R[0][2], R[1][2], 1 + R[2][2]]) + omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) * np.array( + [R[0][2], R[1][2], 1 + R[2][2]] + ) elif not near_zero(1 + R[1][1]): - omg = (1.0 / np.sqrt(2 * (1 + R[1][1]))) \ - * np.array([R[0][1], 1 + R[1][1], R[2][1]]) + omg = (1.0 / np.sqrt(2 * (1 + R[1][1]))) * np.array( + [R[0][1], 1 + R[1][1], R[2][1]] + ) else: - omg = (1.0 / np.sqrt(2 * (1 + R[0][0]))) \ - * np.array([1 + R[0][0], R[1][0], R[2][0]]) + omg = (1.0 / np.sqrt(2 * (1 + R[0][0]))) * np.array( + [1 + R[0][0], R[1][0], R[2][0]] + ) return vector_to_so3(np.pi * omg) else: theta = np.arccos(acos_input) @@ -188,7 +195,7 @@ def SE3_to_Rp(T): np.array([0, 0, 3])) """ T = np.array(T) - return T[0: 3, 0: 3], T[0: 3, 3] + return T[0:3, 0:3], T[0:3, 3] def SE3_inv(T): @@ -227,8 +234,9 @@ def vector_to_se3(V): [-2, 1, 0, 6], [ 0, 0, 0, 0]]) """ - return np.r_[np.c_[vector_to_so3([V[0], V[1], V[2]]), [V[3], V[4], V[5]]], - np.zeros((1, 4))] + return np.r_[ + np.c_[vector_to_so3([V[0], V[1], V[2]]), [V[3], V[4], V[5]]], np.zeros((1, 4)) + ] def se3_to_vec(se3mat): @@ -244,8 +252,10 @@ def se3_to_vec(se3mat): Output: np.array([1, 2, 3, 4, 5, 6]) """ - return np.r_[[se3mat[2][1], se3mat[0][2], se3mat[1][0]], - [se3mat[0][3], se3mat[1][3], se3mat[2][3]]] + return np.r_[ + [se3mat[2][1], se3mat[0][2], se3mat[1][0]], + [se3mat[0][3], se3mat[1][3], se3mat[2][3]], + ] def Adjoint(T): @@ -263,8 +273,7 @@ def Adjoint(T): Chapter 3.3. Rigid-Body Motions and Twists. Pg. 98, Definition 3.20. """ R, p = SE3_to_Rp(T) - return np.r_[np.c_[R, np.zeros((3, 3))], - np.c_[np.dot(vector_to_so3(p), R), R]] + return np.r_[np.c_[R, np.zeros((3, 3))], np.c_[np.dot(vector_to_so3(p), R), R]] def ScrewToAxis(q, s, h): @@ -326,19 +335,25 @@ def matrix_exp6(se3_mat): [ 0, 0, 0, 1]]) """ se3_mat = np.array(se3_mat) - omgtheta = so3_to_vector(se3_mat[0: 3, 0: 3]) + omgtheta = so3_to_vector(se3_mat[0:3, 0:3]) if near_zero(np.linalg.norm(omgtheta)): - return np.r_[np.c_[np.eye(3), se3_mat[0: 3, 3]], [[0, 0, 0, 1]]] + return np.r_[np.c_[np.eye(3), se3_mat[0:3, 3]], [[0, 0, 0, 1]]] else: theta = axis_angle3(omgtheta)[1] - omgmat = se3_mat[0: 3, 0: 3] / theta - return np.r_[np.c_[matrix_exp3(se3_mat[0: 3, 0: 3]), - np.dot(np.eye(3) * theta - + (1 - np.cos(theta)) * omgmat - + (theta - np.sin(theta)) - * np.dot(omgmat, omgmat), - se3_mat[0: 3, 3]) / theta], - [[0, 0, 0, 1]]] + omgmat = se3_mat[0:3, 0:3] / theta + return np.r_[ + np.c_[ + matrix_exp3(se3_mat[0:3, 0:3]), + np.dot( + np.eye(3) * theta + + (1 - np.cos(theta)) * omgmat + + (theta - np.sin(theta)) * np.dot(omgmat, omgmat), + se3_mat[0:3, 3], + ) + / theta, + ], + [[0, 0, 0, 1]], + ] def matrix_log6(T): @@ -360,18 +375,25 @@ def matrix_log6(T): R, p = SE3_to_Rp(T) omgmat = matrix_log3(R) if np.array_equal(omgmat, np.zeros((3, 3))): - return np.r_[np.c_[np.zeros((3, 3)), - [T[0][3], T[1][3], T[2][3]]], - [[0, 0, 0, 0]]] + return np.r_[ + np.c_[np.zeros((3, 3)), [T[0][3], T[1][3], T[2][3]]], [[0, 0, 0, 0]] + ] else: theta = np.arccos((np.trace(R) - 1) / 2.0) - return np.r_[np.c_[omgmat, - np.dot(np.eye(3) - omgmat / 2.0 - + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) - * np.dot(omgmat, omgmat) / theta, [T[0][3], - T[1][3], - T[2][3]])], - [[0, 0, 0, 0]]] + return np.r_[ + np.c_[ + omgmat, + np.dot( + np.eye(3) + - omgmat / 2.0 + + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) + * np.dot(omgmat, omgmat) + / theta, + [T[0][3], T[1][3], T[2][3]], + ), + ], + [[0, 0, 0, 0]], + ] def project_to_SO3(mat): @@ -442,7 +464,7 @@ def distance_to_SO3(mat): if np.linalg.det(mat) > 0: return np.linalg.norm(np.dot(np.array(mat).T, mat) - np.eye(3)) else: - return 1e+9 + return 1e9 def distance_to_SE3(mat): @@ -465,12 +487,17 @@ def distance_to_SE3(mat): Output: 0.134931 """ - matR = np.array(mat)[0: 3, 0: 3] + matR = np.array(mat)[0:3, 0:3] if np.linalg.det(matR) > 0: return np.linalg.norm( - np.r_[np.c_[np.dot(np.transpose(matR), matR), np.zeros((3, 1))], [np.array(mat)[3, :]]] - np.eye(4)) + np.r_[ + np.c_[np.dot(np.transpose(matR), matR), np.zeros((3, 1))], + [np.array(mat)[3, :]], + ] + - np.eye(4) + ) else: - return 1e+9 + return 1e9 def test_if_SO3(mat): @@ -516,9 +543,9 @@ def test_if_SE3(mat): return abs(distance_to_SE3(mat)) < 1e-3 -''' +""" *** CHAPTER 4: FORWARD KINEMATICS *** -''' +""" def fk_in_body(M, Blist, q_list): @@ -578,7 +605,9 @@ def fk_in_space(M, s_list, theta_list): """ T = np.array(M) for i in range(len(theta_list) - 1, -1, -1): - T = np.dot(matrix_exp6(vector_to_se3(np.array(s_list)[:, i] * theta_list[i])), T) + T = np.dot( + matrix_exp6(vector_to_se3(np.array(s_list)[:, i] * theta_list[i])), T + ) return T @@ -607,8 +636,9 @@ def jacobian_body(Blist, q_list): Jb = np.array(Blist).copy().astype(np.float) T = np.eye(4) for i in range(len(q_list) - 2, -1, -1): - T = np.dot(T, matrix_exp6(vector_to_se3(np.array(Blist)[:, i + 1] - * -q_list[i + 1]))) + T = np.dot( + T, matrix_exp6(vector_to_se3(np.array(Blist)[:, i + 1] * -q_list[i + 1])) + ) Jb[:, i] = np.dot(Adjoint(T), np.array(Blist)[:, i]) return Jb @@ -638,14 +668,16 @@ def jacobian_space(S_list, q_list): Js = np.array(S_list).copy().astype(float) T = np.eye(4) for i in range(1, len(q_list)): - T = np.dot(T, matrix_exp6(vector_to_se3(np.array(S_list)[:, i - 1] * q_list[i - 1]))) + T = np.dot( + T, matrix_exp6(vector_to_se3(np.array(S_list)[:, i - 1] * q_list[i - 1])) + ) Js[:, i] = np.dot(Adjoint(T), np.array(S_list)[:, i]) return Js -''' +""" *** CHAPTER 6: INVERSE KINEMATICS *** -''' +""" def ik_in_body(S_body, M, T, q_list0, eomg, ev): @@ -694,20 +726,19 @@ def ik_in_body(S_body, M, T, q_list0, eomg, ev): q_list = np.array(q_list0).copy() i = 0 maxiterations = 20 - Vb = se3_to_vec(matrix_log6(np.dot(SE3_inv(fk_in_body(M, S_body, - q_list)), T))) - err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \ - or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev + Vb = se3_to_vec(matrix_log6(np.dot(SE3_inv(fk_in_body(M, S_body, q_list)), T))) + err = ( + np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg + or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev + ) while err and i < maxiterations: - q_list = q_list \ - + np.dot(np.linalg.pinv(jacobian_body(S_body, - q_list)), Vb) + q_list = q_list + np.dot(np.linalg.pinv(jacobian_body(S_body, q_list)), Vb) i = i + 1 - Vb \ - = se3_to_vec(matrix_log6(np.dot(SE3_inv(fk_in_body(M, S_body, - q_list)), T))) - err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \ - or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev + Vb = se3_to_vec(matrix_log6(np.dot(SE3_inv(fk_in_body(M, S_body, q_list)), T))) + err = ( + np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg + or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev + ) return q_list, not err @@ -758,24 +789,26 @@ def ik_in_space(S_space, M, T, q_list0, eomg, ev): i = 0 maxiterations = 20 Tsb = fk_in_space(M, S_space, q_list) - Vs = np.dot(Adjoint(Tsb), - se3_to_vec(matrix_log6(np.dot(SE3_inv(Tsb), T)))) - err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \ - or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev + Vs = np.dot(Adjoint(Tsb), se3_to_vec(matrix_log6(np.dot(SE3_inv(Tsb), T)))) + err = ( + np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg + or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev + ) while err and i < maxiterations: q_list = q_list + np.dot(np.linalg.pinv(jacobian_space(S_space, q_list)), Vs) i = i + 1 Tsb = fk_in_space(M, S_space, q_list) - Vs = np.dot(Adjoint(Tsb), - se3_to_vec(matrix_log6(np.dot(SE3_inv(Tsb), T)))) - err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \ - or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev + Vs = np.dot(Adjoint(Tsb), se3_to_vec(matrix_log6(np.dot(SE3_inv(Tsb), T)))) + err = ( + np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg + or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev + ) return q_list, not err -''' +""" *** CHAPTER 8: DYNAMICS OF OPEN CHAINS *** -''' +""" def ad(V): @@ -792,8 +825,10 @@ def ad(V): Chapter 8. Dynamics of Open Chains. Pg. 287, Eq. 8.38. """ omega_mat = vector_to_so3([V[0], V[1], V[2]]) - return np.r_[np.c_[omega_mat, np.zeros((3, 3))], - np.c_[vector_to_so3([V[3], V[4], V[5]]), omega_mat]] + return np.r_[ + np.c_[omega_mat, np.zeros((3, 3))], + np.c_[vector_to_so3([V[3], V[4], V[5]]), omega_mat], + ] def InverseDynamics(q_list, dq_list, ddq_list, g, Ftip, Mlist, Glist, Slist): @@ -860,18 +895,23 @@ def InverseDynamics(q_list, dq_list, ddq_list, g, Ftip, Mlist, Glist, Slist): for i in range(n): Mi = np.dot(Mi, Mlist[i]) Ai[:, i] = np.dot(Adjoint(SE3_inv(Mi)), np.array(Slist)[:, i]) - AdTi[i] = Adjoint(np.dot(matrix_exp6(vector_to_se3(Ai[:, i] * - -q_list[i])), - SE3_inv(Mlist[i]))) + AdTi[i] = Adjoint( + np.dot(matrix_exp6(vector_to_se3(Ai[:, i] * -q_list[i])), SE3_inv(Mlist[i])) + ) Vi[:, i + 1] = np.dot(AdTi[i], Vi[:, i]) + Ai[:, i] * dq_list[i] - Vdi[:, i + 1] = np.dot(AdTi[i], Vdi[:, i]) \ - + Ai[:, i] * ddq_list[i] \ - + np.dot(ad(Vi[:, i + 1]), Ai[:, i]) * dq_list[i] + Vdi[:, i + 1] = ( + np.dot(AdTi[i], Vdi[:, i]) + + Ai[:, i] * ddq_list[i] + + np.dot(ad(Vi[:, i + 1]), Ai[:, i]) * dq_list[i] + ) for i in range(n - 1, -1, -1): - Fi = np.dot(np.array(AdTi[i + 1]).T, Fi) \ - + np.dot(np.array(Glist[i]), Vdi[:, i + 1]) \ - - np.dot(np.array(ad(Vi[:, i + 1])).T, - np.dot(np.array(Glist[i]), Vi[:, i + 1])) + Fi = ( + np.dot(np.array(AdTi[i + 1]).T, Fi) + + np.dot(np.array(Glist[i]), Vdi[:, i + 1]) + - np.dot( + np.array(ad(Vi[:, i + 1])).T, np.dot(np.array(Glist[i]), Vi[:, i + 1]) + ) + ) tau_list[i] = np.dot(np.array(Fi).T, Ai[:, i]) return tau_list @@ -927,9 +967,16 @@ def MassMatrix(q_list, Mlist, Glist, Slist): for i in range(n): ddq_list = [0] * n ddq_list[i] = 1 - M[:, i] = InverseDynamics(q_list, [0] * n, ddq_list, - [0, 0, 0], [0, 0, 0, 0, 0, 0], Mlist, - Glist, Slist) + M[:, i] = InverseDynamics( + q_list, + [0] * n, + ddq_list, + [0, 0, 0], + [0, 0, 0, 0, 0, 0], + Mlist, + Glist, + Slist, + ) return M @@ -976,9 +1023,16 @@ def vel_quadratic_forces(q_list, dq_list, Mlist, Glist, Slist): Output: np.array([0.26453118, -0.05505157, -0.00689132]) """ - return InverseDynamics(q_list, dq_list, [0] * len(q_list), - [0, 0, 0], [0, 0, 0, 0, 0, 0], Mlist, Glist, - Slist) + return InverseDynamics( + q_list, + dq_list, + [0] * len(q_list), + [0, 0, 0], + [0, 0, 0, 0, 0, 0], + Mlist, + Glist, + Slist, + ) def GravityForces(q_list, g, Mlist, Glist, Slist): @@ -1024,8 +1078,9 @@ def GravityForces(q_list, g, Mlist, Glist, Slist): np.array([28.40331262, -37.64094817, -5.4415892]) """ n = len(q_list) - return InverseDynamics(q_list, [0] * n, [0] * n, g, - [0, 0, 0, 0, 0, 0], Mlist, Glist, Slist) + return InverseDynamics( + q_list, [0] * n, [0] * n, g, [0, 0, 0, 0, 0, 0], Mlist, Glist, Slist + ) def EndEffectorForces(q_list, Ftip, Mlist, Glist, Slist): @@ -1073,8 +1128,9 @@ def EndEffectorForces(q_list, Ftip, Mlist, Glist, Slist): np.array([1.40954608, 1.85771497, 1.392409]) """ n = len(q_list) - return InverseDynamics(q_list, [0] * n, [0] * n, [0, 0, 0], Ftip, - Mlist, Glist, Slist) + return InverseDynamics( + q_list, [0] * n, [0] * n, [0, 0, 0], Ftip, Mlist, Glist, Slist + ) def ForwardDynamics(q_list, dq_list, tau_list, g, Ftip, Mlist, Glist, Slist): @@ -1126,11 +1182,13 @@ def ForwardDynamics(q_list, dq_list, tau_list, g, Ftip, Mlist, Glist, Slist): Output: np.array([-0.97392907, 25.58466784, -32.91499212]) """ - return np.dot(np.linalg.inv(MassMatrix(q_list, Mlist, Glist, Slist)), - np.array(tau_list) - - vel_quadratic_forces(q_list, dq_list, Mlist, Glist, Slist) - - GravityForces(q_list, g, Mlist, Glist, Slist) - - EndEffectorForces(q_list, Ftip, Mlist, Glist, Slist)) + return np.dot( + np.linalg.inv(MassMatrix(q_list, Mlist, Glist, Slist)), + np.array(tau_list) + - vel_quadratic_forces(q_list, dq_list, Mlist, Glist, Slist) + - GravityForces(q_list, g, Mlist, Glist, Slist) + - EndEffectorForces(q_list, Ftip, Mlist, Glist, Slist), + ) def EulerStep(q_list, dq_list, ddq_list, dt): @@ -1158,8 +1216,7 @@ def EulerStep(q_list, dq_list, ddq_list, dt): return q_list + dt * np.array(dq_list), dq_list + dt * np.array(ddq_list) -def InverseDynamicsTrajectory(q_mat, dq_mat, ddq_mat, g, - Ftipmat, Mlist, Glist, Slist): +def InverseDynamicsTrajectory(q_mat, dq_mat, ddq_mat, g, Ftipmat, Mlist, Glist, Slist): """Calculates the joint forces/torques required to move the serial chain along the given trajectory using inverse dynamics :param q_mat: An N x n matrix of robot joint variables @@ -1251,16 +1308,23 @@ def InverseDynamicsTrajectory(q_mat, dq_mat, ddq_mat, g, Ftipmat = np.array(Ftipmat).T tau_mat = np.array(q_mat).copy() for i in range(np.array(q_mat).shape[1]): - tau_mat[:, i] \ - = InverseDynamics(q_mat[:, i], dq_mat[:, i], - ddq_mat[:, i], g, Ftipmat[:, i], Mlist, - Glist, Slist) + tau_mat[:, i] = InverseDynamics( + q_mat[:, i], + dq_mat[:, i], + ddq_mat[:, i], + g, + Ftipmat[:, i], + Mlist, + Glist, + Slist, + ) tau_mat = np.array(tau_mat).T return tau_mat -def ForwardDynamicsTrajectory(q_list, dq_list, tau_mat, g, Ftipmat, - Mlist, Glist, Slist, dt, intRes): +def ForwardDynamicsTrajectory( + q_list, dq_list, tau_mat, g, Ftipmat, Mlist, Glist, Slist, dt, intRes +): """Simulates the motion of a serial chain given an open-loop history of joint forces/torques. @@ -1366,11 +1430,10 @@ def ForwardDynamicsTrajectory(q_list, dq_list, tau_mat, g, Ftipmat, dq_mat[:, 0] = dq_list for i in range(np.array(tau_mat).shape[1] - 1): for j in range(intRes): - ddq_list \ - = ForwardDynamics(q_list, dq_list, tau_mat[:, i], g, - Ftipmat[:, i], Mlist, Glist, Slist) - q_list, dq_list = EulerStep(q_list, dq_list, - ddq_list, 1.0 * dt / intRes) + ddq_list = ForwardDynamics( + q_list, dq_list, tau_mat[:, i], g, Ftipmat[:, i], Mlist, Glist, Slist + ) + q_list, dq_list = EulerStep(q_list, dq_list, ddq_list, 1.0 * dt / intRes) q_mat[:, i + 1] = q_list dq_mat[:, i + 1] = dq_list q_mat = np.array(q_mat).T @@ -1378,9 +1441,9 @@ def ForwardDynamicsTrajectory(q_list, dq_list, tau_mat, g, Ftipmat, return q_mat, dq_mat -''' +""" *** CHAPTER 9: TRAJECTORY GENERATION *** -''' +""" def CubicTimeScaling(Tf, t): @@ -1507,7 +1570,9 @@ def ScrewTrajectory(x_start, x_end, Tf, N, method): s = CubicTimeScaling(Tf, timegap * i) else: s = QuinticTimeScaling(Tf, timegap * i) - traj[i] = np.dot(x_start, matrix_exp6(matrix_log6(np.dot(SE3_inv(x_start), x_end)) * s)) + traj[i] = np.dot( + x_start, matrix_exp6(matrix_log6(np.dot(SE3_inv(x_start), x_end)) * s) + ) return traj @@ -1568,21 +1633,39 @@ def CartesianTrajectory(x_start, x_end, Tf, N, method): s = CubicTimeScaling(Tf, timegap * i) else: s = QuinticTimeScaling(Tf, timegap * i) - traj[i] \ - = np.r_[np.c_[np.dot(rot_start, - matrix_exp3(matrix_log3(np.dot(np.array(rot_start).T, Rend)) * s)), - s * np.array(pend) + (1 - s) * np.array(p_start)], - [[0, 0, 0, 1]]] + traj[i] = np.r_[ + np.c_[ + np.dot( + rot_start, + matrix_exp3(matrix_log3(np.dot(np.array(rot_start).T, Rend)) * s), + ), + s * np.array(pend) + (1 - s) * np.array(p_start), + ], + [[0, 0, 0, 1]], + ] return traj -''' +""" *** CHAPTER 11: ROBOT CONTROL *** -''' - - -def ComputedTorque(q_list, dq_list, eint, g, Mlist, Glist, Slist, - q_d_list, dq_d_list, ddq_d_list, kp, ki, kd): +""" + + +def ComputedTorque( + q_list, + dq_list, + eint, + g, + Mlist, + Glist, + Slist, + q_d_list, + dq_d_list, + ddq_d_list, + kp, + ki, + kd, +): """Computes the joint control torques at a particular time instant :param q_list: n-vector of joint variables :param dq_list: n-vector of joint rates @@ -1640,16 +1723,34 @@ def ComputedTorque(q_list, dq_list, eint, g, Mlist, Glist, Slist, np.array([133.00525246, -29.94223324, -3.03276856]) """ e = np.subtract(q_d_list, q_list) - return np.dot(MassMatrix(q_list, Mlist, Glist, Slist), - kp * e + ki * (np.array(eint) + e) - + kd * np.subtract(dq_d_list, dq_list)) + InverseDynamics(q_list, dq_list, ddq_d_list, g, - [0, 0, 0, 0, 0, 0], Mlist, Glist, - Slist) - - -def simulate_control(q_list, dq_list, g, tip_force_mat, Mlist, Glist, - Slist, q_d_mat, dq_d_mat, ddq_d_mat, g_tilde, - Mtildelist, Gtildelist, kp, ki, kd, dt, intRes): + return np.dot( + MassMatrix(q_list, Mlist, Glist, Slist), + kp * e + ki * (np.array(eint) + e) + kd * np.subtract(dq_d_list, dq_list), + ) + InverseDynamics( + q_list, dq_list, ddq_d_list, g, [0, 0, 0, 0, 0, 0], Mlist, Glist, Slist + ) + + +def simulate_control( + q_list, + dq_list, + g, + tip_force_mat, + Mlist, + Glist, + Slist, + q_d_mat, + dq_d_mat, + ddq_d_mat, + g_tilde, + Mtildelist, + Gtildelist, + kp, + ki, + kd, + dt, + intRes, +): """Simulates the computed torque controller over a given desired trajectory. @@ -1776,21 +1877,41 @@ def simulate_control(q_list, dq_list, g, tip_force_mat, Mlist, Glist, m, n = np.array(q_d_mat).shape q_current = np.array(q_list).copy() dq_current = np.array(dq_list).copy() - eint = np.zeros((m, 1)).reshape(m, ) + eint = np.zeros((m, 1)).reshape( + m, + ) tau_mat = np.zeros(np.array(q_d_mat).shape) q_mat = np.zeros(np.array(q_d_mat).shape) for i in range(n): - tau_list \ - = ComputedTorque(q_current, dq_current, eint, g_tilde, - Mtildelist, Gtildelist, Slist, q_d_mat[:, i], - dq_d_mat[:, i], ddq_d_mat[:, i], kp, ki, kd) + tau_list = ComputedTorque( + q_current, + dq_current, + eint, + g_tilde, + Mtildelist, + Gtildelist, + Slist, + q_d_mat[:, i], + dq_d_mat[:, i], + ddq_d_mat[:, i], + kp, + ki, + kd, + ) for j in range(intRes): - ddq_list \ - = ForwardDynamics(q_current, dq_current, tau_list, g, - tip_force_mat[:, i], Mlist, Glist, Slist) - q_current, dq_current \ - = EulerStep(q_current, dq_current, ddq_list, - 1.0 * dt / intRes) + ddq_list = ForwardDynamics( + q_current, + dq_current, + tau_list, + g, + tip_force_mat[:, i], + Mlist, + Glist, + Slist, + ) + q_current, dq_current = EulerStep( + q_current, dq_current, ddq_list, 1.0 * dt / intRes + ) tau_mat[:, i] = tau_list q_mat[:, i] = q_current eint = np.add(eint, dt * np.subtract(q_d_mat[:, i], q_current)) @@ -1799,20 +1920,33 @@ def simulate_control(q_list, dq_list, g, tip_force_mat, Mlist, Glist, import matplotlib.pyplot as plt except ImportError: plt = None - print('The result will not be plotted due to a lack of package matplotlib') + print("The result will not be plotted due to a lack of package matplotlib") else: links = np.array(q_mat).shape[0] N = np.array(q_mat).shape[1] Tf = N * dt timestamp = np.linspace(0, Tf, N) for i in range(links): - col = [np.random.uniform(0, 1), np.random.uniform(0, 1), - np.random.uniform(0, 1)] - plt.plot(timestamp, q_mat[i, :], "-", color=col, - label=("ActualTheta" + str(i + 1))) - plt.plot(timestamp, q_d_mat[i, :], ".", color=col, - label=("DesiredTheta" + str(i + 1))) - plt.legend(loc='upper left') + col = [ + np.random.uniform(0, 1), + np.random.uniform(0, 1), + np.random.uniform(0, 1), + ] + plt.plot( + timestamp, + q_mat[i, :], + "-", + color=col, + label=("ActualTheta" + str(i + 1)), + ) + plt.plot( + timestamp, + q_d_mat[i, :], + ".", + color=col, + label=("DesiredTheta" + str(i + 1)), + ) + plt.legend(loc="upper left") plt.xlabel("Time") plt.ylabel("Joint Angles") plt.title("Plot of Actual and Desired Joint Angles") @@ -1834,16 +1968,26 @@ def mecanum_base_get_wheel_velocities(base_vel, wheel_radius, width, length): Returns: ndarray [vel_fl, vel_fr, vel_bl, vel_br] """ - radius_reciprocal = 1. / wheel_radius - angular_factor = (width + length) / 2. * base_vel.angular.z - vel_fl = radius_reciprocal * (base_vel.linear.x - base_vel.linear.y - angular_factor) - vel_fr = radius_reciprocal * (base_vel.linear.x + base_vel.linear.y + angular_factor) - vel_bl = radius_reciprocal * (base_vel.linear.x + base_vel.linear.y - angular_factor) - vel_br = radius_reciprocal * (base_vel.linear.x - base_vel.linear.y + angular_factor) + radius_reciprocal = 1.0 / wheel_radius + angular_factor = (width + length) / 2.0 * base_vel.angular.z + vel_fl = radius_reciprocal * ( + base_vel.linear.x - base_vel.linear.y - angular_factor + ) + vel_fr = radius_reciprocal * ( + base_vel.linear.x + base_vel.linear.y + angular_factor + ) + vel_bl = radius_reciprocal * ( + base_vel.linear.x + base_vel.linear.y - angular_factor + ) + vel_br = radius_reciprocal * ( + base_vel.linear.x - base_vel.linear.y + angular_factor + ) return np.array([vel_fl, vel_fr, vel_bl, vel_br]) -def mecanum_base_get_base_vel(vel_fl, vel_fr, vel_bl, vel_br, wheel_radius, width, length): +def mecanum_base_get_base_vel( + vel_fl, vel_fr, vel_bl, vel_br, wheel_radius, width, length +): """Given velocities for the four wheels, get the base velocity. Args: @@ -1859,7 +2003,9 @@ def mecanum_base_get_base_vel(vel_fl, vel_fr, vel_bl, vel_br, wheel_radius, widt Base velocity in Twist. """ twist = Twist() - twist.linear.x = (vel_fl + vel_fr + vel_bl + vel_br) * (wheel_radius / 4.) - twist.linear.y = (-vel_fl + vel_fr + vel_bl - vel_br) * (wheel_radius / 4.) - twist.angular.z = (-vel_fl + vel_fr - vel_bl + vel_br) * (wheel_radius / (2. * (width + length))) + twist.linear.x = (vel_fl + vel_fr + vel_bl + vel_br) * (wheel_radius / 4.0) + twist.linear.y = (-vel_fl + vel_fr + vel_bl - vel_br) * (wheel_radius / 4.0) + twist.angular.z = (-vel_fl + vel_fr - vel_bl + vel_br) * ( + wheel_radius / (2.0 * (width + length)) + ) return twist diff --git a/src/rotools/utility/transform.py b/src/rotools/utility/transform.py index 8c708b4..a729c78 100644 --- a/src/rotools/utility/transform.py +++ b/src/rotools/utility/transform.py @@ -251,15 +251,19 @@ def rotation_matrix(angle, direction, point=None): cosa = math.cos(angle) direction = unit_vector(direction[:3]) # rotation matrix around unit vector - R = numpy.array(((cosa, 0.0, 0.0), - (0.0, cosa, 0.0), - (0.0, 0.0, cosa)), dtype=numpy.float64) + R = numpy.array( + ((cosa, 0.0, 0.0), (0.0, cosa, 0.0), (0.0, 0.0, cosa)), dtype=numpy.float64 + ) R += numpy.outer(direction, direction) * (1.0 - cosa) direction *= sina - R += numpy.array(((0.0, -direction[2], direction[1]), - (direction[2], 0.0, -direction[0]), - (-direction[1], direction[0], 0.0)), - dtype=numpy.float64) + R += numpy.array( + ( + (0.0, -direction[2], direction[1]), + (direction[2], 0.0, -direction[0]), + (-direction[1], direction[0], 0.0), + ), + dtype=numpy.float64, + ) M = numpy.identity(4) M[:3, :3] = R if point is not None: @@ -323,10 +327,15 @@ def scale_matrix(factor, origin=None, direction=None): """ if direction is None: # uniform scaling - M = numpy.array(((factor, 0.0, 0.0, 0.0), - (0.0, factor, 0.0, 0.0), - (0.0, 0.0, factor, 0.0), - (0.0, 0.0, 0.0, 1.0)), dtype=numpy.float64) + M = numpy.array( + ( + (factor, 0.0, 0.0, 0.0), + (0.0, factor, 0.0, 0.0), + (0.0, 0.0, factor, 0.0), + (0.0, 0.0, 0.0, 1.0), + ), + dtype=numpy.float64, + ) if origin is not None: M[:3, 3] = origin[:3] M[:3, 3] *= 1.0 - factor @@ -380,8 +389,7 @@ def scale_from_matrix(matrix): return factor, origin, direction -def projection_matrix(point, normal, direction=None, - perspective=None, pseudo=False): +def projection_matrix(point, normal, direction=None, perspective=None, pseudo=False): """Return matrix to project onto plane defined by point and normal. Using either perspective point, projection direction, or none of both. If pseudo is True, perspective projections will preserve relative depth @@ -413,8 +421,7 @@ def projection_matrix(point, normal, direction=None, normal = unit_vector(normal[:3]) if perspective is not None: # perspective projection - perspective = numpy.array(perspective[:3], dtype=numpy.float64, - copy=False) + perspective = numpy.array(perspective[:3], dtype=numpy.float64, copy=False) M[0, 0] = M[1, 1] = M[2, 2] = numpy.dot(perspective - point, normal) M[:3, :3] -= numpy.outer(perspective, normal) if pseudo: @@ -497,11 +504,10 @@ def projection_from_matrix(matrix, pseudo=False): # perspective projection i = numpy.where(abs(numpy.real(l)) > 1e-8)[0] if not len(i): - raise ValueError( - "no eigenvector not corresponding to eigenvalue 0") + raise ValueError("no eigenvector not corresponding to eigenvalue 0") point = numpy.real(V[:, i[-1]]).squeeze() point /= point[3] - normal = - M[3, :3] + normal = -M[3, :3] perspective = M[:3, 3] / numpy.dot(point[:3], normal) if pseudo: perspective -= normal @@ -542,15 +548,19 @@ def clip_matrix(left, right, bottom, top, near, far, perspective=False): if near <= _EPS: raise ValueError("invalid frustrum: near <= 0") t = 2.0 * near - M = ((-t / (right - left), 0.0, (right + left) / (right - left), 0.0), - (0.0, -t / (top - bottom), (top + bottom) / (top - bottom), 0.0), - (0.0, 0.0, -(far + near) / (far - near), t * far / (far - near)), - (0.0, 0.0, -1.0, 0.0)) + M = ( + (-t / (right - left), 0.0, (right + left) / (right - left), 0.0), + (0.0, -t / (top - bottom), (top + bottom) / (top - bottom), 0.0), + (0.0, 0.0, -(far + near) / (far - near), t * far / (far - near)), + (0.0, 0.0, -1.0, 0.0), + ) else: - M = ((2.0 / (right - left), 0.0, 0.0, (right + left) / (left - right)), - (0.0, 2.0 / (top - bottom), 0.0, (top + bottom) / (bottom - top)), - (0.0, 0.0, 2.0 / (far - near), (far + near) / (near - far)), - (0.0, 0.0, 0.0, 1.0)) + M = ( + (2.0 / (right - left), 0.0, 0.0, (right + left) / (left - right)), + (0.0, 2.0 / (top - bottom), 0.0, (top + bottom) / (bottom - top)), + (0.0, 0.0, 2.0 / (far - near), (far + near) / (near - far)), + (0.0, 0.0, 0.0, 1.0), + ) return numpy.array(M, dtype=numpy.float64) @@ -704,8 +714,9 @@ def decompose_matrix(matrix): return scale, shear, angles, translate, perspective -def compose_matrix(scale=None, shear=None, angles=None, translate=None, - perspective=None): +def compose_matrix( + scale=None, shear=None, angles=None, translate=None, perspective=None +): """Return transformation matrix from sequence of transformations. This is the inverse of the decompose_matrix math. Sequence of transformations: @@ -735,7 +746,7 @@ def compose_matrix(scale=None, shear=None, angles=None, translate=None, T[:3, 3] = translate[:3] M = numpy.dot(M, T) if angles is not None: - R = euler_matrix(angles[0], angles[1], angles[2], 'sxyz') + R = euler_matrix(angles[0], angles[1], angles[2], "sxyz") M = numpy.dot(M, R) if shear is not None: Z = numpy.identity(4) @@ -769,12 +780,15 @@ def orthogonalization_matrix(lengths, angles): sina, sinb, _ = numpy.sin(angles) cosa, cosb, cosg = numpy.cos(angles) co = (cosa * cosb - cosg) / (sina * sinb) - return numpy.array(( - (a * sinb * math.sqrt(1.0 - co * co), 0.0, 0.0, 0.0), - (-a * sinb * co, b * sina, 0.0, 0.0), - (a * cosb, b * cosa, c, 0.0), - (0.0, 0.0, 0.0, 1.0)), - dtype=numpy.float64) + return numpy.array( + ( + (a * sinb * math.sqrt(1.0 - co * co), 0.0, 0.0, 0.0), + (-a * sinb * co, b * sina, 0.0, 0.0), + (a * cosb, b * cosa, c, 0.0), + (0.0, 0.0, 0.0, 1.0), + ), + dtype=numpy.float64, + ) def superimposition_matrix(v0, v1, scaling=False, usesvd=True): @@ -848,10 +862,12 @@ def superimposition_matrix(v0, v1, scaling=False, usesvd=True): xx, yy, zz = numpy.sum(v0 * v1, axis=1) xy, yz, zx = numpy.sum(v0 * numpy.roll(v1, -1, axis=0), axis=1) xz, yx, zy = numpy.sum(v0 * numpy.roll(v1, -2, axis=0), axis=1) - N = ((xx + yy + zz, yz - zy, zx - xz, xy - yx), - (yz - zy, xx - yy - zz, xy + yx, zx + xz), - (zx - xz, xy + yx, -xx + yy - zz, yz + zy), - (xy - yx, zx + xz, yz + zy, -xx - yy + zz)) + N = ( + (xx + yy + zz, yz - zy, zx - xz, xy - yx), + (yz - zy, xx - yy - zz, xy + yx, zx + xz), + (zx - xz, xy + yx, -xx + yy - zz, yz + zy), + (xy - yx, zx + xz, yz + zy, -xx - yy + zz), + ) # quaternion: eigenvector corresponding to most positive eigenvalue l, V = numpy.linalg.eig(N) q = V[:, numpy.argmax(l)] @@ -874,7 +890,7 @@ def superimposition_matrix(v0, v1, scaling=False, usesvd=True): return M -def euler_matrix(ai, aj, ak, axes='sxyz'): +def euler_matrix(ai, aj, ak, axes="sxyz"): """Return homogeneous rotation matrix (4x4) from Euler angles and axis sequence. ai, aj, ak : Euler's roll, pitch and yaw angles axes : One of 24 axis sequences as string or encoded tuple @@ -934,7 +950,7 @@ def euler_matrix(ai, aj, ak, axes='sxyz'): return M -def euler_from_matrix(matrix, axes='sxyz'): +def euler_from_matrix(matrix, axes="sxyz"): """Return Euler angles from rotation matrix for specified axis sequence. axes : One of 24 axis sequences as string or encoded tuple Note that many Euler angle triplets can describe one matrix. @@ -988,7 +1004,7 @@ def euler_from_matrix(matrix, axes='sxyz'): return ax, ay, az -def euler_from_quaternion(quaternion, axes='sxyz'): +def euler_from_quaternion(quaternion, axes="sxyz"): """Return Euler angles from quaternion for specified axis sequence. >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947]) >>> numpy.allclose(angles, [0.123, 0, 0]) @@ -997,7 +1013,7 @@ def euler_from_quaternion(quaternion, axes='sxyz'): return euler_from_matrix(quaternion_matrix(quaternion), axes) -def quaternion_from_euler(ai, aj, ak, axes='sxyz'): +def quaternion_from_euler(ai, aj, ak, axes="sxyz"): """Return quaternion from Euler angles and axis sequence. ai, aj, ak : Euler's roll, pitch and yaw angles axes : One of 24 axis sequences as string or encoded tuple @@ -1078,12 +1094,15 @@ def quaternion_matrix(quaternion): return numpy.identity(4) q *= math.sqrt(2.0 / nq) q = numpy.outer(q, q) - return numpy.array(( - (1.0 - q[1, 1] - q[2, 2], q[0, 1] - q[2, 3], q[0, 2] + q[1, 3], 0.0), - (q[0, 1] + q[2, 3], 1.0 - q[0, 0] - q[2, 2], q[1, 2] - q[0, 3], 0.0), - (q[0, 2] - q[1, 3], q[1, 2] + q[0, 3], 1.0 - q[0, 0] - q[1, 1], 0.0), - (0.0, 0.0, 0.0, 1.0) - ), dtype=numpy.float64) + return numpy.array( + ( + (1.0 - q[1, 1] - q[2, 2], q[0, 1] - q[2, 3], q[0, 2] + q[1, 3], 0.0), + (q[0, 1] + q[2, 3], 1.0 - q[0, 0] - q[2, 2], q[1, 2] - q[0, 3], 0.0), + (q[0, 2] - q[1, 3], q[1, 2] + q[0, 3], 1.0 - q[0, 0] - q[1, 1], 0.0), + (0.0, 0.0, 0.0, 1.0), + ), + dtype=numpy.float64, + ) def quaternion_from_matrix(matrix): @@ -1125,11 +1144,15 @@ def quaternion_multiply(quaternion1, quaternion0): """ x0, y0, z0, w0 = quaternion0 x1, y1, z1, w1 = quaternion1 - return numpy.array(( - x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, - -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, - x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0, - -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0), dtype=numpy.float64) + return numpy.array( + ( + x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, + -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, + x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0, + -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0, + ), + dtype=numpy.float64, + ) def quaternion_conjugate(quaternion): @@ -1139,8 +1162,10 @@ def quaternion_conjugate(quaternion): >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3]) True """ - return numpy.array((-quaternion[0], -quaternion[1], - -quaternion[2], quaternion[3]), dtype=numpy.float64) + return numpy.array( + (-quaternion[0], -quaternion[1], -quaternion[2], quaternion[3]), + dtype=numpy.float64, + ) def quaternion_inverse(quaternion): @@ -1213,10 +1238,15 @@ def random_quaternion(rand=None): pi2 = math.pi * 2.0 t1 = pi2 * rand[1] t2 = pi2 * rand[2] - return numpy.array((numpy.sin(t1) * r1, - numpy.cos(t1) * r1, - numpy.sin(t2) * r2, - numpy.cos(t2) * r2), dtype=numpy.float64) + return numpy.array( + ( + numpy.sin(t1) * r1, + numpy.cos(t1) * r1, + numpy.sin(t2) * r2, + numpy.cos(t2) * r2, + ), + dtype=numpy.float64, + ) def random_rotation_matrix(rand=None): @@ -1343,9 +1373,10 @@ def matrix(self): def arcball_map_to_sphere(point, center, radius): """Return unit sphere coordinates from window coordinates.""" - v = numpy.array(((point[0] - center[0]) / radius, - (center[1] - point[1]) / radius, - 0.0), dtype=numpy.float64) + v = numpy.array( + ((point[0] - center[0]) / radius, (center[1] - point[1]) / radius, 0.0), + dtype=numpy.float64, + ) n = v[0] * v[0] + v[1] * v[1] if n > 1.0: v /= math.sqrt(n) # position outside of sphere @@ -1391,14 +1422,31 @@ def arcball_nearest_axis(point, axes): # map axes strings to/from tuples of inner axis, parity, repetition, frame _AXES2TUPLE = { - 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), - 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), - 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), - 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), - 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), - 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), - 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), - 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} + "sxyz": (0, 0, 0, 0), + "sxyx": (0, 0, 1, 0), + "sxzy": (0, 1, 0, 0), + "sxzx": (0, 1, 1, 0), + "syzx": (1, 0, 0, 0), + "syzy": (1, 0, 1, 0), + "syxz": (1, 1, 0, 0), + "syxy": (1, 1, 1, 0), + "szxy": (2, 0, 0, 0), + "szxz": (2, 0, 1, 0), + "szyx": (2, 1, 0, 0), + "szyz": (2, 1, 1, 0), + "rzyx": (0, 0, 0, 1), + "rxyx": (0, 0, 1, 1), + "ryzx": (0, 1, 0, 1), + "rxzx": (0, 1, 1, 1), + "rxzy": (1, 0, 0, 1), + "ryzy": (1, 0, 1, 1), + "rzxy": (1, 1, 0, 1), + "ryxy": (1, 1, 1, 1), + "ryxz": (2, 0, 0, 1), + "rzxz": (2, 0, 1, 1), + "rxyz": (2, 1, 0, 1), + "rzyz": (2, 1, 1, 1), +} _TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items()) @@ -1537,7 +1585,7 @@ def is_same_transform(matrix0, matrix1): return numpy.allclose(matrix0, matrix1) -def _import_module(module_name, warn=True, prefix='_py_', ignore='_'): +def _import_module(module_name, warn=True, prefix="_py_", ignore="_"): """Try import all public attributes from module into global namespace. Existing attributes with name clashes are renamed with prefix. Attributes starting with underscore are ignored by default. diff --git a/src/rotools/websocket/core/client.py b/src/rotools/websocket/core/client.py index 09ee151..c5b5486 100644 --- a/src/rotools/websocket/core/client.py +++ b/src/rotools/websocket/core/client.py @@ -2,6 +2,7 @@ import json from uuid import uuid4 + # If using python 2.7: # sudo pip install websocket-client # If using python 3+, you will also need: @@ -23,26 +24,36 @@ def __init__(self, kwargs): Class to manage publishing to ROS through a ros-bridge websocket. """ self._advertise_dict = {} - rospy.loginfo("Connecting to websocket: {}:{}".format(kwargs['ip'], kwargs['port'])) - self.ws = websocket.create_connection('ws://' + kwargs['ip'] + ':' + str(kwargs['port']), timeout=4) + rospy.loginfo( + "Connecting to websocket: {}:{}".format(kwargs["ip"], kwargs["port"]) + ) + self.ws = websocket.create_connection( + "ws://" + kwargs["ip"] + ":" + str(kwargs["port"]), timeout=4 + ) # Down streams: the client subscribe to local topics and publish it to the server - from_client_key = 'from_client_topics' + from_client_key = "from_client_topics" if from_client_key in kwargs: if kwargs[from_client_key] is not None: self.local_subscribers = [] self.downstream_list = kwargs[from_client_key] for i, entity in enumerate(self.downstream_list): local_topic_id, remote_topic_id, msg_type = entity - subscriber = self.create_subscriber(local_topic_id, msg_type, remote_topic_id) + subscriber = self.create_subscriber( + local_topic_id, msg_type, remote_topic_id + ) self.local_subscribers.append(subscriber) else: - rospy.loginfo('{} is empty'.format(from_client_key)) + rospy.loginfo("{} is empty".format(from_client_key)) else: - rospy.logwarn('{} is not presented in the config dict {}'.format(from_client_key, kwargs)) + rospy.logwarn( + "{} is not presented in the config dict {}".format( + from_client_key, kwargs + ) + ) # Up stream: the client receive topics from the server and publish them locally - to_client_key = 'to_client_topics' + to_client_key = "to_client_topics" if to_client_key in kwargs: if kwargs[to_client_key] is not None: self.local_timers = [] @@ -54,20 +65,28 @@ def __init__(self, kwargs): self.local_timers.append(timer) self.local_publishers.append(publisher) else: - rospy.loginfo('{} is empty'.format(to_client_key)) + rospy.loginfo("{} is empty".format(to_client_key)) else: - rospy.logwarn('{} is not presented in the config dict {}'.format(to_client_key, kwargs)) + rospy.logwarn( + "{} is not presented in the config dict {}".format( + to_client_key, kwargs + ) + ) def create_subscriber(self, local_topic_id, msg_type, remote_topic_id): - if 'PoseStamped' in msg_type: - return rospy.Subscriber(local_topic_id, PoseStamped, self._pose_stamped_cb, remote_topic_id) - elif 'Twist' in msg_type: - return rospy.Subscriber(local_topic_id, Twist, self._twist_cb, remote_topic_id) + if "PoseStamped" in msg_type: + return rospy.Subscriber( + local_topic_id, PoseStamped, self._pose_stamped_cb, remote_topic_id + ) + elif "Twist" in msg_type: + return rospy.Subscriber( + local_topic_id, Twist, self._twist_cb, remote_topic_id + ) else: raise NotImplementedError def create_timer(self, local_topic_id, msg_type): - if 'Odometry' in msg_type: + if "Odometry" in msg_type: timer = rospy.Timer(rospy.Duration.from_sec(0.001), self._odometry_cb) publisher = rospy.Publisher(local_topic_id, Odometry, queue_size=1) return timer, publisher @@ -88,8 +107,8 @@ def _twist_cb(self, msg, remote_topic_id): def _odometry_cb(self, event): for i, entity in enumerate(self.upstream_list): local_topic_id, remote_topic_id, msg_type = entity - if 'Odometry' in msg_type: - result = self.subscribe_once(remote_topic_id, 'nav_msgs/Odometry') + if "Odometry" in msg_type: + result = self.subscribe_once(remote_topic_id, "nav_msgs/Odometry") self.local_publishers[i].publish(result) def _advertise(self, topic_name, topic_type): @@ -103,21 +122,25 @@ def _advertise(self, topic_name, topic_type): str: ID to de-advertise later on. """ new_uuid = str(uuid4()) - self._advertise_dict[new_uuid] = {'topic_name': topic_name, - 'topic_type': topic_type} - advertise_msg = {"op": "advertise", - "id": new_uuid, - "topic": topic_name, - "type": topic_type - } + self._advertise_dict[new_uuid] = { + "topic_name": topic_name, + "topic_type": topic_type, + } + advertise_msg = { + "op": "advertise", + "id": new_uuid, + "topic": topic_name, + "type": topic_type, + } self.ws.send(json.dumps(advertise_msg)) return new_uuid def _un_advertise(self, uuid): - unad_msg = {"op": "unadvertise", - "id": uuid, - # "topic": topic_name - } + unad_msg = { + "op": "unadvertise", + "id": uuid, + # "topic": topic_name + } self.ws.send(json.dumps(unad_msg)) def __del__(self): @@ -136,11 +159,7 @@ def _publish(self, topic_name, message): Returns: """ - msg = { - 'op': 'publish', - 'topic': topic_name, - 'msg': message - } + msg = {"op": "publish", "topic": topic_name, "msg": message} json_msg = json.dumps(msg) self.ws.send(json_msg) @@ -157,7 +176,7 @@ def publish(self, topic_name, ros_message): # First check if we already advertised the topic d = self._advertise_dict for k in d: - if d[k]['topic_name'] == topic_name: + if d[k]["topic_name"] == topic_name: # Already advertised, do nothing break else: @@ -178,16 +197,14 @@ def subscribe_once(self, topic_name, msg_type): Returns: result: ROS message """ - msg = { - 'op': 'subscribe', - 'topic': topic_name, - 'type': msg_type - } + msg = {"op": "subscribe", "topic": topic_name, "type": msg_type} json_msg = json.dumps(msg) self.ws.send(json_msg) json_message = self.ws.recv() - dictionary = json.loads(json_message)['msg'] - result = message_converter.convert_dictionary_to_ros_message(msg_type, dictionary) + dictionary = json.loads(json_message)["msg"] + result = message_converter.convert_dictionary_to_ros_message( + msg_type, dictionary + ) # print("Type: '%s' \n Received: '%s'" % (type, result)) return result diff --git a/src/rotools/xsens/core/interface.py b/src/rotools/xsens/core/interface.py index b571c9a..319340e 100644 --- a/src/rotools/xsens/core/interface.py +++ b/src/rotools/xsens/core/interface.py @@ -36,23 +36,43 @@ def __init__(self, header): self.payload_size = header[9] # Size of the measurement excluding the header def __repr__(self): - s = 'Header {}: \nsample_counter {}, datagram_counter {},\n' \ - 'item #{}, body segment #{}, prop #{}, finger segment #{}\n'.format( - self.ID_str, self.sample_counter, self.datagram_counter, self.item_counter, - self.body_segments_num, self.props_num, self.finger_segments_num) + s = ( + "Header {}: \nsample_counter {}, datagram_counter {},\n" + "item #{}, body segment #{}, prop #{}, finger segment #{}\n".format( + self.ID_str, + self.sample_counter, + self.datagram_counter, + self.item_counter, + self.body_segments_num, + self.props_num, + self.finger_segments_num, + ) + ) return s @property def is_valid(self): - if self.ID_str != 'MXTP02': - rospy.logwarn('XSensInterface: Current only support MXTP02, but got {}'.format(self.ID_str)) + if self.ID_str != "MXTP02": + rospy.logwarn( + "XSensInterface: Current only support MXTP02, but got {}".format( + self.ID_str + ) + ) return False - if self.item_counter != self.body_segments_num + self.props_num + self.finger_segments_num: - rospy.logwarn('XSensInterface: Segments number in total does not match item counter') + if ( + self.item_counter + != self.body_segments_num + self.props_num + self.finger_segments_num + ): + rospy.logwarn( + "XSensInterface: Segments number in total does not match item counter" + ) return False if self.payload_size % self.item_counter != 0: - rospy.logwarn('XSensInterface: Payload size {} is not dividable by item number {}'.format( - self.payload_size, self.item_num)) + rospy.logwarn( + "XSensInterface: Payload size {} is not dividable by item number {}".format( + self.payload_size, self.item_num + ) + ) return False return True @@ -72,11 +92,7 @@ def item_size(self): class Datagram(object): - def __init__( - self, - header, - payload - ): + def __init__(self, header, payload): self.header = header self.payload = payload @@ -84,7 +100,9 @@ def __init__( def is_object(self): return self.header.is_object - def decode_to_pose_array_msg(self, ref_frame, ref_frame_id=None, scaling_factor=1.0): + def decode_to_pose_array_msg( + self, ref_frame, ref_frame_id=None, scaling_factor=1.0 + ): """Decode the bytes in the streaming data to pose array message. :param ref_frame: str Reference frame name of the generated pose array message. @@ -98,7 +116,9 @@ def decode_to_pose_array_msg(self, ref_frame, ref_frame_id=None, scaling_factor= pose_array_msg.header.frame_id = ref_frame for i in range(self.header.item_num): - item = self.payload[i * self.header.item_size:(i + 1) * self.header.item_size] + item = self.payload[ + i * self.header.item_size : (i + 1) * self.header.item_size + ] pose_msg = self._decode_to_pose_msg(item) if pose_msg is None: return None @@ -122,7 +142,9 @@ def _decode_to_pose_msg(item): :param item: str String of bytes """ if len(item) != 32: - rospy.logerr('XSensInterface: Payload pose data size is not 32: {}'.format(len(item))) + rospy.logerr( + "XSensInterface: Payload pose data size is not 32: {}".format(len(item)) + ) return None # segment_id = common.byte_to_uint32(item[:4]) x = common.byte_to_float(item[4:8]) @@ -141,13 +163,13 @@ def _decode_to_pose_msg(item): class XsensInterface(object): def __init__( - self, - udp_ip, - udp_port, - ref_frame, - scaling=1.0, - buffer_size=4096, - **kwargs # DO NOT REMOVE + self, + udp_ip, + udp_port, + ref_frame, + scaling=1.0, + buffer_size=4096, + **kwargs # DO NOT REMOVE ): super(XsensInterface, self).__init__() @@ -156,26 +178,45 @@ def __init__( self._buffer_size = buffer_size self._body_frames = { - 'pelvis': 0, - 'l5': 1, 'l3': 2, - 't12': 3, 't8': 4, - 'neck': 5, 'head': 6, - 'right_shoulder': 7, 'right_upper_arm': 8, 'right_forearm': 9, 'right_hand': 10, - 'left_shoulder': 11, 'left_upper_arm': 12, 'left_forearm': 13, 'left_hand': 14, - 'right_upper_leg': 15, 'right_lower_leg': 16, 'right_foot': 17, 'right_toe': 18, - 'left_upper_leg': 19, 'left_lower_leg': 20, 'left_foot': 21, 'left_toe': 22, + "pelvis": 0, + "l5": 1, + "l3": 2, + "t12": 3, + "t8": 4, + "neck": 5, + "head": 6, + "right_shoulder": 7, + "right_upper_arm": 8, + "right_forearm": 9, + "right_hand": 10, + "left_shoulder": 11, + "left_upper_arm": 12, + "left_forearm": 13, + "left_hand": 14, + "right_upper_leg": 15, + "right_lower_leg": 16, + "right_foot": 17, + "right_toe": 18, + "left_upper_leg": 19, + "left_lower_leg": 20, + "left_foot": 21, + "left_toe": 22, } ref_frame_lowercase = ref_frame.lower() if ref_frame_lowercase in self._body_frames: self.ref_frame = ref_frame_lowercase self.ref_frame_id = self._body_frames[ref_frame_lowercase] - elif ref_frame_lowercase == '' or ref_frame_lowercase == 'world': - rospy.logwarn('XSensInterface: Reference frame is the world frame') - self.ref_frame = 'world' + elif ref_frame_lowercase == "" or ref_frame_lowercase == "world": + rospy.logwarn("XSensInterface: Reference frame is the world frame") + self.ref_frame = "world" self.ref_frame_id = None else: - rospy.logwarn('XSensInterface: Using customized reference frame {}'.format(ref_frame_lowercase)) + rospy.logwarn( + "XSensInterface: Using customized reference frame {}".format( + ref_frame_lowercase + ) + ) self.ref_frame = ref_frame_lowercase self.ref_frame_id = None @@ -195,13 +236,13 @@ def first_object_pose(self): return pose def get_datagram(self): - """[Main entrance function] Get poses from the datagram. - - """ + """[Main entrance function] Get poses from the datagram.""" data, _ = self._sock.recvfrom(self._buffer_size) datagram = self._get_datagram(data) if datagram is not None: - pose_array_msg = datagram.decode_to_pose_array_msg(self.ref_frame, self.ref_frame_id) + pose_array_msg = datagram.decode_to_pose_array_msg( + self.ref_frame, self.ref_frame_id + ) if pose_array_msg is None: return False if datagram.is_object: @@ -238,8 +279,9 @@ def get_body_poses(self): # all_poses should at least contain body segment poses segment_id = 0 - assert len(self.body_segments_poses.poses) >= 23, \ - rospy.logerr("XSensInterface: Body segments' number is less than 23") + assert len(self.body_segments_poses.poses) >= 23, rospy.logerr( + "XSensInterface: Body segments' number is less than 23" + ) for p in self.body_segments_poses.poses: main_body_msg.poses.append(p) if segment_id == 4: # T8 @@ -258,8 +300,14 @@ def get_body_poses(self): if segment_id == self.header.body_segments_num: break assert len(main_body_msg.poses) == self.header.body_segments_num - return [self.body_segments_poses, main_body_msg], \ - [base_pose_msg, left_tcp_msg, right_tcp_msg, left_sole_msg, right_sole_msg, head_msg] + return [self.body_segments_poses, main_body_msg], [ + base_pose_msg, + left_tcp_msg, + right_tcp_msg, + left_sole_msg, + right_sole_msg, + head_msg, + ] def get_transform(self, base_frame, distal_frame): """Given both base frame and distal frame be two of the body frames, get the transform @@ -277,10 +325,12 @@ def get_transform(self, base_frame, distal_frame): distal_frame = distal_frame.lower() pose_msg.header = self.body_segments_poses.header if base_frame not in self._body_frames or distal_frame not in self._body_frames: - rospy.logerr('Base frame {} is not in known body frames'.format(base_frame)) + rospy.logerr("Base frame {} is not in known body frames".format(base_frame)) return None if distal_frame not in self._body_frames: - rospy.logerr('Distal frame {} is not in known body frames'.format(distal_frame)) + rospy.logerr( + "Distal frame {} is not in known body frames".format(distal_frame) + ) return None base_frame_id = self._body_frames[base_frame] distal_frame_id = self._body_frames[distal_frame] @@ -298,7 +348,9 @@ def get_prop_msgs(self): prop_1_msg.pose = self.body_segments_poses.poses[24] return prop_1_msg except IndexError: - rospy.logwarn_once('XSensInterface: Prop number is not 0 but failed to get its pose') + rospy.logwarn_once( + "XSensInterface: Prop number is not 0 but failed to get its pose" + ) return None else: return None @@ -315,7 +367,11 @@ def get_hand_joint_states(self): rospy.loginfo_once("XSensInterface: Finger data is not used") return None, None elif self.header.finger_segments_num != 40: - rospy.logwarn_once("XSensInterface: Finger segment # is not 40: {}".format(self.header.finger_segments_num)) + rospy.logwarn_once( + "XSensInterface: Finger segment # is not 40: {}".format( + self.header.finger_segments_num + ) + ) return None, None left_hand_js = sensor_msg.JointState() @@ -328,10 +384,14 @@ def get_hand_joint_states(self): base_num = self.header.body_segments_num + self.header.props_num for i in [1, 4, 8, 12, 16]: meta_id = base_num + i - left_hand_js.position.extend(self._get_finger_j1_j2(self.body_segments_poses.poses, meta_id)) + left_hand_js.position.extend( + self._get_finger_j1_j2(self.body_segments_poses.poses, meta_id) + ) for i in [1, 4, 8, 12, 16]: meta_id = base_num + 20 + i - right_hand_js.position.extend(self._get_finger_j1_j2(self.body_segments_poses.poses, meta_id)) + right_hand_js.position.extend( + self._get_finger_j1_j2(self.body_segments_poses.poses, meta_id) + ) return left_hand_js, right_hand_js @staticmethod @@ -347,10 +407,14 @@ def _get_finger_j1_j2(poses, meta_id, upper=1.57): m_pose = poses[meta_id] pp_pose = poses[meta_id + 1] dp_pose = poses[meta_id + 2] - _, euler_angles = common.get_relative_rotation(m_pose.orientation, pp_pose.orientation) + _, euler_angles = common.get_relative_rotation( + m_pose.orientation, pp_pose.orientation + ) # * -1 is to convert to walker conversion j1 = np.fabs(euler_angles[np.argmax(np.abs(euler_angles))]) - _, euler_angles = common.get_relative_rotation(pp_pose.orientation, dp_pose.orientation) + _, euler_angles = common.get_relative_rotation( + pp_pose.orientation, dp_pose.orientation + ) j2 = np.fabs(euler_angles[np.argmax(np.abs(euler_angles))]) return [np.minimum(j1, upper) / upper, np.minimum(j2, upper) / upper] @@ -362,11 +426,13 @@ def _get_header(data): :return: Header """ if len(data) < 24: - rospy.logwarn('XSensInterface: Data length {} is less than 24'.format(len(data))) + rospy.logwarn( + "XSensInterface: Data length {} is less than 24".format(len(data)) + ) return None id_str = common.byte_to_str(data[0:6], 6) sample_counter = common.byte_to_uint32(data[6:10]) - datagram_counter = struct.unpack('!B', data[10]) + datagram_counter = struct.unpack("!B", data[10]) item_number = common.byte_to_uint8(data[11]) time_code = common.byte_to_uint32(data[12:16]) character_id = common.byte_to_uint8(data[16]) @@ -375,8 +441,20 @@ def _get_header(data): finger_segments_num = common.byte_to_uint8(data[19]) # 20 21 are reserved for future use payload_size = common.byte_to_uint16(data[22:24]) - header = Header([id_str, sample_counter, datagram_counter, item_number, time_code, character_id, - body_segments_num, props_num, finger_segments_num, payload_size]) + header = Header( + [ + id_str, + sample_counter, + datagram_counter, + item_number, + time_code, + character_id, + body_segments_num, + props_num, + finger_segments_num, + payload_size, + ] + ) rospy.logdebug(header.__repr__()) return header diff --git a/src/rotools/xsens/core/server.py b/src/rotools/xsens/core/server.py index fb4c3b4..73a1040 100644 --- a/src/rotools/xsens/core/server.py +++ b/src/rotools/xsens/core/server.py @@ -22,36 +22,63 @@ def __init__(self, kwargs): super(XsensServer, self).__init__() # Publisher switch - self.srv_pub_switch = rospy.Service('/xsens/enable', SetBool, self.pub_switch_handle) - print_warn('Use [rosservice call /xsens/enable "data: true"] to enable receiving XSens data.') + self.srv_pub_switch = rospy.Service( + "/xsens/enable", SetBool, self.pub_switch_handle + ) + print_warn( + 'Use [rosservice call /xsens/enable "data: true"] to enable receiving XSens data.' + ) self.interface = interface.XsensInterface(**kwargs) # Cartesian pose publishers - self.all_poses_publisher = rospy.Publisher('/xsens/all_poses', PoseArray, queue_size=1) - self.body_poses_publisher = rospy.Publisher('/xsens/body_poses', PoseArray, queue_size=1) - self.pose_array_publishers = [self.all_poses_publisher, self.body_poses_publisher] - - self.base_pose_publisher = rospy.Publisher('/xsens/base', PoseStamped, queue_size=1) - self.left_tcp_publisher = rospy.Publisher('/xsens/left_tcp', PoseStamped, queue_size=1) - self.right_tcp_publisher = rospy.Publisher('/xsens/right_tcp', PoseStamped, queue_size=1) - self.left_sole_publisher = rospy.Publisher('/xsens/left_sole', PoseStamped, queue_size=1) - self.right_sole_publisher = rospy.Publisher('/xsens/right_sole', PoseStamped, queue_size=1) - self.head_publisher = rospy.Publisher('/xsens/head', PoseStamped, queue_size=1) + self.all_poses_publisher = rospy.Publisher( + "/xsens/all_poses", PoseArray, queue_size=1 + ) + self.body_poses_publisher = rospy.Publisher( + "/xsens/body_poses", PoseArray, queue_size=1 + ) + self.pose_array_publishers = [ + self.all_poses_publisher, + self.body_poses_publisher, + ] + + self.base_pose_publisher = rospy.Publisher( + "/xsens/base", PoseStamped, queue_size=1 + ) + self.left_tcp_publisher = rospy.Publisher( + "/xsens/left_tcp", PoseStamped, queue_size=1 + ) + self.right_tcp_publisher = rospy.Publisher( + "/xsens/right_tcp", PoseStamped, queue_size=1 + ) + self.left_sole_publisher = rospy.Publisher( + "/xsens/left_sole", PoseStamped, queue_size=1 + ) + self.right_sole_publisher = rospy.Publisher( + "/xsens/right_sole", PoseStamped, queue_size=1 + ) + self.head_publisher = rospy.Publisher("/xsens/head", PoseStamped, queue_size=1) self.core_poses_publishers = ( - self.base_pose_publisher, self.left_tcp_publisher, self.right_tcp_publisher, - self.left_sole_publisher, self.right_sole_publisher, self.head_publisher, + self.base_pose_publisher, + self.left_tcp_publisher, + self.right_tcp_publisher, + self.left_sole_publisher, + self.right_sole_publisher, + self.head_publisher, ) - self.pub_prop = kwargs['prop'] + self.pub_prop = kwargs["prop"] # Prop pose publisher - self.prop_1_publisher = rospy.Publisher('/xsens/prop_1', PoseStamped, queue_size=1) + self.prop_1_publisher = rospy.Publisher( + "/xsens/prop_1", PoseStamped, queue_size=1 + ) # Customized poses publishers - self.customized_poses = rospy.get_param('~customized_poses', []) + self.customized_poses = rospy.get_param("~customized_poses", []) if self.customized_poses: - rospy.loginfo('Defined customized poses: {}'.format(self.customized_poses)) + rospy.loginfo("Defined customized poses: {}".format(self.customized_poses)) self.customized_poses_publishers = [] for i, entity in enumerate(self.customized_poses): try: @@ -59,22 +86,28 @@ def __init__(self, kwargs): publisher = rospy.Publisher(topic_id, PoseStamped, queue_size=1) self.customized_poses_publishers.append(publisher) except ValueError as e: - rospy.logerr('Entity {}: {}'.format(i, e)) + rospy.logerr("Entity {}: {}".format(i, e)) # Hand joint states publishers - self.left_hand_publisher = rospy.Publisher('/xsens/left_hand_js', JointState, queue_size=1) - self.right_hand_publisher = rospy.Publisher('/xsens/right_hand_js', JointState, queue_size=1) + self.left_hand_publisher = rospy.Publisher( + "/xsens/left_hand_js", JointState, queue_size=1 + ) + self.right_hand_publisher = rospy.Publisher( + "/xsens/right_hand_js", JointState, queue_size=1 + ) # Tracked object publishers - self.object_pose_publisher = rospy.Publisher('/xsens/object', PoseStamped, queue_size=1) + self.object_pose_publisher = rospy.Publisher( + "/xsens/object", PoseStamped, queue_size=1 + ) - rate = kwargs['rate'] - self.all_poses_msg_timer = rospy.Timer(rospy.Duration.from_sec(1.0 / rate), self.all_poses_msg_handle) + rate = kwargs["rate"] + self.all_poses_msg_timer = rospy.Timer( + rospy.Duration.from_sec(1.0 / rate), self.all_poses_msg_handle + ) def all_poses_msg_handle(self, event): - """ - - """ + """ """ if not self.enabled: return if not self.interface.get_datagram(): @@ -117,8 +150,8 @@ def pub_customized_poses(self): def pub_switch_handle(self, req): if req.data: self.set_status(True) - msg = 'Xsens stream receiving enabled' + msg = "Xsens stream receiving enabled" else: self.set_status(False) - msg = 'Xsens stream receiving disabled' + msg = "Xsens stream receiving disabled" return SetBoolResponse(True, msg) diff --git a/srv/ExecuteAllLockedPoses.srv b/srv/ExecuteAllLockedPoses.srv new file mode 100644 index 0000000..8d9f178 --- /dev/null +++ b/srv/ExecuteAllLockedPoses.srv @@ -0,0 +1,23 @@ +# Move multiple groups to the target poses. The first group will be used as the reference, whose goal pose is given, +# other groups will move according to the reference and kept the relative pose along movement. + +# Requests +# group_names: Names of the moving groups. +# goal_type: GLOBAL: Absolute pose wrt the reference frame (usually the robot base frame). +# LOCAL_ALIGNED: Relative pose wrt the reference frame transferred to current eef base position. +# LOCAL: pose wrt the eef frame. +# goal: Goal pose for the reference group (the first group in group_names). +# stamp: Time interval for all groups reaching goals. + +std_msgs/Header header +string[] group_names +uint8 GLOBAL=0 +uint8 LOCAL_ALIGNED=1 +uint8 LOCAL=2 +uint8 goal_type +geometry_msgs/Pose goal +float64 stamp +--- +uint8 SUCCEEDED=0 +uint8 FAILED=1 +uint8 result_status \ No newline at end of file