Examples: Dynamic frames¶
This document shows how to use dynamic frames.
If you want to see more about dynamic frames functions, you can look at API - Dynamic frames
Danger
If you are using the real robot, make sure the environment around it is clear.
Simple dynamic frame control¶
This example shows how to create a frame and do a small pick and place in this frame:
#!/usr/bin/env python
# Imports
from niryo_robot_python_ros_wrapper import *
import rospy
gripper_speed = 400
# Initializing ROS node
rospy.init_node('niryo_example_python_ros_wrapper')
# Connecting to the ROS Wrapper & calibrating if needed
niryo_robot = NiryoRosWrapper()
niryo_robot.calibrate_auto()
# Create frame
point_o = [0.15, 0.15, 0]
point_x = [0.25, 0.2, 0]
point_y = [0.2, 0.25, 0]
niryo_robot.save_dynamic_frame_from_points("dynamic_frame", "description", [point_o, point_x, point_y])
# Get list of frames
print(niryo_robot.get_saved_dynamic_frame_list())
# Check creation of the frame
info = niryo_robot.get_saved_dynamic_frame("dynamic_frame")
print(info)
# Pick
#niryo_robot.open_gripper(gripper_speed)
# Move to the frame
niryo_robot.move_pose(0, 0, 0, 0, 1.57, 0, "dynamic_frame")
#niryo_robot.close_gripper(gripper_speed)
# Move in frame
niryo_robot.move_linear_relative([0, 0, 0.1, 0, 0, 0], "dynamic_frame")
niryo_robot.move_relative([0.1, 0, 0, 0, 0, 0], "dynamic_frame")
niryo_robot.move_linear_relative([0, 0, -0.1, 0, 0, 0], "dynamic_frame")
# Place
#niryo_robot.open_gripper(gripper_speed)
niryo_robot.move_linear_relative([0, 0, 0.1, 0, 0, 0], "dynamic_frame")
# Home
niryo_robot.move_joints(0, 0.5, -1.25, 0, 0, 0)
# Delete frame
niryo_robot.delete_dynamic_frame("dynamic_frame")