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)
