Пример #1
0
 def __init__(self):
     super().__init__()
     self.victor = Victor()
     self.gripper_points_service = rospy.Service(
         "get_dual_gripper_points", GetDualGripperPoints,
         self.get_dual_gripper_points_cb)
     self.tf_wrapper = TF2Wrapper()
def main():
    rospy.init_node("manual_motion")

    use_left_arm = rospy.get_param("~use_left_arm", True)
    use_right_arm = rospy.get_param("~use_right_arm", True)

    victor = Victor()

    if use_left_arm:
        rospy.loginfo("initializing left arm ...")
        victor.set_left_arm_control_mode(ControlMode.JOINT_IMPEDANCE)
        rospy.loginfo("done")
    else:
        rospy.loginfo("not using left arm")

    if use_right_arm:
        rospy.loginfo("initializing right arm ...", )
        victor.set_right_arm_control_mode(ControlMode.JOINT_IMPEDANCE)
        rospy.loginfo("done")
    else:
        rospy.loginfo("not using right arm")

    left = None
    right = None
    if use_left_arm:
        left = ManualMotion("left_arm")
    if use_right_arm:
        right = ManualMotion("right_arm")

    rospy.on_shutdown(lambda: print_joints(left, right))

    rospy.loginfo("Running")
    rospy.spin()
def main():
    ros_init.rospy_and_cpp_init("test_jacobain_follower")

    v = Victor()
    v.plan_to_pose("left_arm",
                   "left_tool_placeholder",
                   target_pose=[0.7, 0.3, 0.8, np.pi, 0, 0])
    current_pose = v.get_link_pose("left_arm", "left_tool_placeholder")
    for pos in xy_spiral(current_pose.position, radius=0.5, n_steps=200):
        v.follow_jacobian_to_position("left_arm", ['left_tool_placeholder'],
                                      [[pos]],
                                      vel_scaling=0.05)
    ros_init.shutdown()
def get_moveit_robot(robot_namespace: Optional[str] = None):
    default = 'victor'
    if robot_namespace is None:
        if rospy.has_param("robot_namespace"):
            robot_namespace = rospy.get_param("robot_namespace")
        elif rospy.get_namespace() != "/":
            robot_namespace = rospy.get_namespace().strip("/")
        else:
            rospy.logwarn(f"using default robot_namespace {default}")
            robot_namespace = default

    if robot_namespace == 'victor':
        return Victor(robot_namespace)
    elif robot_namespace in ['val', 'hdt_michigan']:
        return Val(robot_namespace)
    else:
        raise NotImplementedError(
            f"robot with namespace {robot_namespace} not implemented")
Пример #5
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()
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()
Пример #7
0
 def __init__(self):
     super().__init__('victor')
     # FIXME: lazy construction
     self.victor = Victor()
Пример #8
0
class DualArmRealVictorRopeScenario(BaseDualArmRopeScenario):
    COLOR_IMAGE_TOPIC = "/kinect2_victor_head/qhd/image_color_rect"
    DEPTH_IMAGE_TOPIC = "/kinect2_victor_head/qhd/image_depth_rect"

    def __init__(self):
        super().__init__('victor')
        # FIXME: lazy construction
        self.victor = Victor()

    def on_before_data_collection(self, params: Dict):
        left_res, right_res = self.victor.base_victor.set_control_mode(
            ControlMode.JOINT_IMPEDANCE)
        if not left_res.success or not right_res.success:
            raise RuntimeError("Failed to switch into impedance mode")

        current_joint_positions = np.array(
            self.robot.get_joint_positions(self.robot.get_both_arm_joints()))
        near_start = np.max(
            np.abs(
                np.array(params['reset_joint_config']) -
                current_joint_positions)) < 0.02
        grippers_are_closed = self.victor.base_victor.is_left_gripper_closed(
        ) and self.victor.base_victor.is_right_gripper_closed()
        if not near_start or not grippers_are_closed:
            # let go
            self.robot.open_left_gripper()

            # move to init positions
            self.robot.plan_to_joint_config("both_arms",
                                            params['reset_joint_config'])

            self.robot.speak("press enter to close grippers")
            input("press enter to close grippers")

        self.robot.speak("press enter to begin")
        input("press enter to begin")

    def on_after_data_collection(self, params):
        self.victor.speak(
            "Phew, I'm done. That was a lot of work! I feel like I've learned so much already."
        )

    def get_state(self):
        # TODO: this should be composed of function calls to get_state for arm_no_rope and get_state for rope?
        joint_state = self.robot.joint_state_listener.get()

        left_gripper_position, right_gripper_position = self.robot.get_gripper_positions(
        )

        color_depth_cropped = self.get_rgbd()

        # rope_state_vector = self.get_rope_state()

        return {
            'joint_positions': joint_state.position,
            'joint_names': joint_state.name,
            'left_gripper': ros_numpy.numpify(left_gripper_position),
            'right_gripper': ros_numpy.numpify(right_gripper_position),
            'rgbd': color_depth_cropped,
            # 'rope': np.array(rope_state_vector, np.float32),
        }

    def states_description(self) -> Dict:
        n_joints = self.robot.get_n_joints()
        return {
            'left_gripper': 3,
            'right_gripper': 3,
            # 'rope': FloatingRopeScenario.n_links * 3,
            'joint_positions': n_joints,
            'rgbd': self.IMAGE_H * self.IMAGE_W * 4,
        }
Пример #9
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")