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()
示例#2
0
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()
示例#3
0
This moves Victor's arms to a configuration where impedance validations tends to work well.
if you want to actually change the control mode, not just move to the position, set "_actually_switch:=true"
"""
import colorama
from colorama import Fore

import rospy
from arm_robots.victor import Victor

if __name__ == "__main__":
    colorama.init(autoreset=True)
    rospy.init_node("move_to_impedance_switch")
    rospy.logwarn("Make sure you set up a conservative set of obstacles in gazebo")
    rospy.logwarn("View the planning scene in RViz to see what the planner is aware of")
    k = input("Did you set up a conservative set of obstacles in gazebo? [y/N]")
    if k == 'Y' or k == 'y':
        rospy.loginfo(Fore.CYAN + "you better not be lying...")

        victor = Victor(robot_namespace='victor')
        victor.connect()

        actually_switch = rospy.get_param("~actually_switch", False)
        if actually_switch:
            rospy.loginfo("switching to impedance mode")

        victor.move_to_impedance_switch(actually_switch=actually_switch)

        rospy.loginfo("Done")
    else:
        rospy.loginfo("Answered 'no', aborting")