Exemple #1
0
 def trajectory_from_data(traj_data):
     try:
         return JointTrajectory.from_data(traj_data)
     except AttributeError:
         return [
             Frame.from_data(frame_data) for frame_data in traj_data
         ]
 def data(self, data):
     self.frame = Frame.from_data(data['frame'])
     if '_tool_frame' in data:
         self.tool_frame = Frame.from_data(data['_tool_frame'])
     if '_source' in data:
         self._source = _deserialize_from_data(data['_source'])
     if '_mesh' in data:
         #self._mesh = _deserialize_from_data(data['_mesh'])
         self._mesh = Mesh.from_data(data['_mesh'])
     if 'trajectory' in data:
         from compas_fab.robots import JointTrajectory
         self.trajectory = [
             JointTrajectory.from_data(d) for d in data['trajectory']
         ]
         #self.trajectory = _deserialize_from_data(data['trajectory'])
     if 'path' in data:
         self.path = [Frame.from_data(d) for d in data['path']]
def test_serialization(trj):
    data = trj.to_data()
    new_trj = JointTrajectory.from_data(data)
    assert (new_trj.to_data() == data)
    assert (new_trj.time_from_start == Duration(6, 0).seconds)
def main():
    choreo_problem_instance_dir = compas_fab.get('choreo_instances')
    result_save_path = os.path.join(choreo_problem_instance_dir, 'results',
                                    'choreo_result.json')
    with open(result_save_path, 'r') as f:
        json_data = json.loads(f.read())

    traj_time_cnt = 0.0
    max_jt_vel = 0.2
    max_jt_acc = 0.1
    last_jt_pt = None

    # pybullet traj preview settings
    pybullet_preview = True
    PB_VIZ_CART_TIME_STEP = 0.05
    PB_VIZ_TRANS_TIME_STEP = 0.04
    PB_VIZ_PER_CONF_SIM = False

    urdf_filename = compas_fab.get(
        'universal_robot/ur_description/urdf/ur5.urdf')
    srdf_filename = compas_fab.get(
        'universal_robot/ur5_moveit_config/config/ur5.srdf')
    urdf_pkg_name = 'ur_description'

    ee_filename = compas_fab.get(
        'universal_robot/ur_description/meshes/' +
        'pychoreo_workshop_gripper/collision/victor_gripper_jaw03.obj')

    # [0.0, -94.94770102010436, 98.0376624092449, -93.01855212389889, 0.0, 0.0]
    # UR=192.168.0.30, Linux=192.168.0.1, Windows=192.168.0.2
    # the following host IP should agree with the Linux machine
    host_ip = '192.168.0.1' if REAL_EXECUTION else 'localhost'
    with RosClient(host=host_ip, port=9090) as client:
        client.on_ready(
            lambda: print('Is ROS connected?', client.is_connected))

        # get current configuration
        listener = roslibpy.Topic(client, JOINT_TOPIC_NAME,
                                  'sensor_msgs/JointState')
        msg_getter = MsgGetter()
        listener.subscribe(msg_getter.receive_msg)
        time.sleep(2)
        last_seen_state = msg_getter.get_msg()
        print('current jt state: {}'.format(last_seen_state['position']))

        model = RobotModel.from_urdf_file(urdf_filename)
        semantics = RobotSemantics.from_srdf_file(srdf_filename, model)
        robot = RobotClass(model, semantics=semantics, client=client)
        group = robot.main_group_name
        joint_names = robot.get_configurable_joint_names()
        # base_link_name = robot.get_base_link_name()
        ee_link_name = robot.get_end_effector_link_name()

        if pybullet_preview:
            from conrob_pybullet import connect
            from compas_fab.backends.ros.plugins_choreo import display_trajectory_chunk
            from compas_fab.backends.pybullet import attach_end_effector_geometry, \
            convert_mesh_to_pybullet_body, create_pb_robot_from_ros_urdf

            connect(use_gui=True)
            pb_robot = create_pb_robot_from_ros_urdf(urdf_filename,
                                                     urdf_pkg_name,
                                                     ee_link_name=ee_link_name)
            ee_meshes = [Mesh.from_obj(ee_filename)]
            ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot,
                                                      ee_link_name)

        st_conf = Configuration.from_revolute_values(
            last_seen_state['position'])
        goal_conf = Configuration.from_revolute_values(
            json_data[0]['place2pick']['start_configuration']['values'])
        goal_constraints = robot.constraints_from_configuration(
            goal_conf, [math.radians(1)] * 6, group)
        init_traj_raw = robot.plan_motion(goal_constraints,
                                          st_conf,
                                          group,
                                          planner_id='RRTStar')
        init_traj = traj_reparam(init_traj_raw,
                                 max_jt_vel,
                                 max_jt_acc,
                                 inspect_sol=False)

        if pybullet_preview:
            display_trajectory_chunk(pb_robot, joint_names,
                                     init_traj.to_data(), \
                                     ee_attachs=ee_attachs, grasped_attach=[],
                                     time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)

        print('************\nexecuting init transition')
        exec_jt_traj(client, joint_names, init_traj)

        print('executed?')
        input()

        for seq_id, e_process_data in enumerate(json_data):
            print('************\nexecuting #{} picknplace process'.format(
                seq_id))

            # open gripper
            gripper_srv_call(client, state=0)

            print(
                '=====\nexecuting #{} place-retreat to pick-approach transition process'
                .format(seq_id))
            traj_data = e_process_data['place2pick']
            ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
            ros_jt_traj = traj_reparam(ros_jt_traj_raw,
                                       max_jt_vel,
                                       max_jt_acc,
                                       inspect_sol=False)

            if pybullet_preview:
                display_trajectory_chunk(pb_robot, joint_names,
                                        ros_jt_traj.to_data(), \
                                        ee_attachs=ee_attachs, grasped_attach=[],
                                        time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
            exec_jt_traj(client, joint_names, ros_jt_traj)

            print('=====\nexecuting #{} pick-approach to pick-grasp process'.
                  format(seq_id))
            traj_data = e_process_data['pick_approach']
            ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
            ros_jt_traj = traj_reparam(ros_jt_traj_raw,
                                       max_jt_vel,
                                       max_jt_acc,
                                       inspect_sol=False)
            if pybullet_preview:
                display_trajectory_chunk(pb_robot, joint_names,
                                        ros_jt_traj.to_data(), \
                                        ee_attachs=ee_attachs, grasped_attach=[],
                                        time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)

            exec_jt_traj(client, joint_names, ros_jt_traj)

            print('executed?')
            input()

            # close gripper
            gripper_srv_call(client, state=1)

            print('=====\nexecuting #{} pick-grasp to pick-retreat process'.
                  format(seq_id))
            traj_data = e_process_data['pick_retreat']
            ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
            ros_jt_traj = traj_reparam(ros_jt_traj_raw,
                                       max_jt_vel,
                                       max_jt_acc,
                                       inspect_sol=False)
            if pybullet_preview:
                display_trajectory_chunk(pb_robot, joint_names,
                                        ros_jt_traj.to_data(), \
                                        ee_attachs=ee_attachs, grasped_attach=[],
                                        time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)

            exec_jt_traj(client, joint_names, ros_jt_traj)

            print('executed?')
            input()

            print(
                '=====\nexecuting #{} pick-retreat to place-approach transition process'
                .format(seq_id))
            traj_data = e_process_data['pick2place']
            ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
            ros_jt_traj = traj_reparam(ros_jt_traj_raw,
                                       max_jt_vel,
                                       max_jt_acc,
                                       inspect_sol=False)
            if pybullet_preview:
                display_trajectory_chunk(pb_robot, joint_names,
                                        ros_jt_traj.to_data(), \
                                        ee_attachs=ee_attachs, grasped_attach=[],
                                        time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)

            exec_jt_traj(client, joint_names, ros_jt_traj)

            print('executed?')
            input()

            print('=====\nexecuting #{} place-approach to place-grasp process'.
                  format(seq_id))
            traj_data = e_process_data['place_approach']
            ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
            ros_jt_traj = traj_reparam(ros_jt_traj_raw,
                                       max_jt_vel,
                                       max_jt_acc,
                                       inspect_sol=False)
            if pybullet_preview:
                display_trajectory_chunk(pb_robot, joint_names,
                                        ros_jt_traj.to_data(), \
                                        ee_attachs=ee_attachs, grasped_attach=[],
                                        time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)

            exec_jt_traj(client, joint_names, ros_jt_traj)

            # open gripper
            gripper_srv_call(client, state=0)

            print('executed?')
            input()

            print('=====\nexecuting #{} place-grasp to place-retreat process'.
                  format(seq_id))
            traj_data = e_process_data['place_retreat']
            ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
            ros_jt_traj = traj_reparam(ros_jt_traj_raw,
                                       max_jt_vel,
                                       max_jt_acc,
                                       inspect_sol=False)
            if pybullet_preview:
                display_trajectory_chunk(pb_robot, joint_names,
                                        ros_jt_traj.to_data(), \
                                        ee_attachs=ee_attachs, grasped_attach=[],
                                        time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
            exec_jt_traj(client, joint_names, ros_jt_traj)

            print('executed?')
            input()

        if 'return2idle' in json_data[-1]:
            print('=====\nexecuting #{} return-to-idle transition process'.
                  format(seq_id))
            traj_data = e_process_data['return2idle']
            ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
            ros_jt_traj = traj_reparam(ros_jt_traj_raw,
                                       max_jt_vel,
                                       max_jt_acc,
                                       inspect_sol=False)
            if pybullet_preview:
                display_trajectory_chunk(pb_robot, joint_names,
                                        ros_jt_traj.to_data(), \
                                        ee_attachs=ee_attachs, grasped_attach=[],
                                        time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
            exec_jt_traj(client, joint_names, ros_jt_traj)