def main(): np.set_printoptions(suppress=True, precision=0, linewidth=200) colorama.init(autoreset=True) rospy_and_cpp_init("basic_motion") victor = Victor() victor.connect() victor.open_left_gripper() rospy.sleep(2) victor.close_left_gripper() rospy.sleep(2) victor.open_right_gripper() rospy.sleep(2) victor.close_right_gripper() rospy.sleep(2) print("press enter if prompted") # # Plan to joint config myinput("Plan to joint config?") victor.plan_to_joint_config(victor.right_arm_group, [0.35, 1, 0.2, -1, 0.2, -1, 0]) # # # Plan to pose myinput("Plan to pose 1?") victor.plan_to_pose(victor.right_arm_group, victor.right_tool_name, [0.6, -0.2, 1.0, 4, 1, 0]) # Or you can use a geometry msgs Pose myinput("Plan to pose 2?") pose = Pose() pose.position.x = 0.7 pose.position.y = -0.2 pose.position.z = 1.0 q = quaternion_from_euler(np.pi, 0, 0) pose.orientation.x = q[0] pose.orientation.y = q[1] pose.orientation.z = q[2] pose.orientation.w = q[3] victor.plan_to_pose(victor.right_arm_group, victor.right_tool_name, pose) # # Or with cartesian planning myinput("Cartersian motion back to pose 3?") victor.plan_to_position_cartesian(victor.right_arm_group, victor.right_tool_name, [0.9, -0.4, 0.9], step_size=0.01) victor.plan_to_position_cartesian(victor.right_arm_group, victor.right_tool_name, [0.7, -0.4, 0.9], step_size=0.01) # Move hand straight works either with jacobian following myinput("Follow jacobian to pose 2?") victor.store_current_tool_orientations([victor.right_tool_name]) victor.follow_jacobian_to_position(victor.right_arm_group, [victor.right_tool_name], [[[0.7, -0.4, 0.6]]]) victor.follow_jacobian_to_position(victor.right_arm_group, [victor.right_tool_name], [[[0.8, -0.4, 1.0]]]) victor.follow_jacobian_to_position(victor.right_arm_group, [victor.right_tool_name], [[[1.1, -0.4, 0.9]]]) victor.follow_jacobian_to_position(group_name=victor.right_arm_group, tool_names=[victor.right_tool_name], preferred_tool_orientations=[quaternion_from_euler(np.pi, 0, 0)], points=[[[1.1, -0.2, 0.8]]]) roscpp_initializer.shutdown()
def main(): np.set_printoptions(suppress=True, precision=0, linewidth=200) colorama.init(autoreset=True) rospy_and_cpp_init("victor_ipython") victor = Victor() victor.connect() victor.close_left_gripper() victor.open_left_gripper() victor.close_right_gripper() victor.open_right_gripper() ipdb.set_trace() roscpp_initializer.shutdown()