def data(self, data):
self.points = list(
map(JointTrajectoryPoint.from_data,
data.get('points') or []))
self.start_configuration = Configuration.from_data(
data.get('start_configuration'))
self.fraction = data.get('fraction')
def get_robot_configuration(self, robot, group):
robot_uid = robot.attributes['pybullet_uid']
joint_names = robot.get_configurable_joint_names(group=group)
joints = joints_from_names(robot_uid, joint_names)
joint_types = robot.get_joint_types_by_names(joint_names)
joint_values = get_joint_positions(robot_uid, joints)
return Configuration(values=joint_values,
types=joint_types,
joint_names=joint_names)
def get_configuration(self, group=None):
"""Returns the current configuration.
"""
positions = []
types = []
for joint in self.get_configurable_joints(group):
positions.append(joint.position)
types.append(joint.type)
return Configuration(positions, types)
def visualize_urscript(script):
M = [
[-1000, 0, 0, 0],
[0, 1000, 0, 0],
[0, 0, 1000, 0],
[0, 0, 0, 1],
]
rgT = matrix_to_rgtransform(M)
cgT = Transformation.from_matrix(M)
robot = Robot()
viz_planes = []
movel_matcher = re.compile(r"^\s*move([lj]).+((-?\d+\.\d+,?\s?){6}).*$")
for line in script.splitlines():
mo = re.search(movel_matcher, line)
if mo:
if mo.group(1) == "l": # movel
ptX, ptY, ptZ, rX, rY, rZ = mo.group(2).split(",")
pt = Point(float(ptX), float(ptY), float(ptZ))
pt.transform(cgT)
frame = Frame(pt, [1, 0, 0], [0, 1, 0])
R = Rotation.from_axis_angle_vector(
[float(rX), float(rY), float(rZ)], pt)
T = matrix_to_rgtransform(R)
plane = cgframe_to_rgplane(frame)
plane.Transform(T)
viz_planes.append(plane)
else: # movej
joint_values = mo.group(2).split(",")
configuration = Configuration.from_revolute_values(
[float(d) for d in joint_values])
frame = robot.forward_kinematics(configuration)
plane = cgframe_to_rgplane(frame)
plane.Transform(rgT)
viz_planes.append(plane)
return viz_planes
def get_group_configuration(self, group, full_configuration):
"""Returns the group's configuration.
Parameters
----------
group : str
The name of the group.
full_configuration : :class:`compas_fab.robots.Configuration`
The configuration for all configurable joints of the robot.
Returns
-------
:class:`compas_fab.robots.Configuration`
"""
values = []
types = []
group_joint_names = self.get_configurable_joint_names(group)
for i, name in enumerate(self.get_configurable_joint_names()):
if name in group_joint_names:
types.append(full_configuration.types[i])
values.append(full_configuration.values[i])
return Configuration(values, types)
def main():
parser = argparse.ArgumentParser()
# ur_picknplace_multiple_piece
parser.add_argument('-p',
'--problem',
default='ur_picknplace_single_piece',
help='The name of the problem to solve')
parser.add_argument('-rob',
'--robot',
default='ur3',
help='The type of UR robot to use.')
parser.add_argument('-m',
'--plan_transit',
action='store_false',
help='Plans motions between each picking and placing')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
parser.add_argument('-s',
'--save_result',
action='store_true',
help='save planning results as a json file')
parser.add_argument(
'-scale',
'--model_scale',
default=0.001,
help='model scale conversion to meter, default 0.001 (from millimeter)'
)
parser.add_argument('-vik',
'--view_ikfast',
action='store_true',
help='Visualize each ikfast solutions')
parser.add_argument('-tres',
'--transit_res',
default=0.01,
help='joint resolution (rad)')
parser.add_argument('-ros',
'--use_ros',
action='store_true',
help='use ros backend with moveit planners')
parser.add_argument('-cart_ts',
'--cartesian_time_step',
default=0.1,
help='cartesian time step in trajectory simulation')
parser.add_argument('-trans_ts',
'--transit_time_step',
default=0.01,
help='transition time step in trajectory simulation')
parser.add_argument('-per_conf_step',
'--per_conf_step',
action='store_true',
help='stepping each configuration in simulation')
args = parser.parse_args()
print('Arguments:', args)
VIZ = args.viewer
VIZ_IKFAST = args.view_ikfast
TRANSITION_JT_RESOLUTION = float(args.transit_res)
plan_transition = args.plan_transit
use_moveit_planner = args.use_ros
# sim settings
CART_TIME_STEP = args.cartesian_time_step
TRANSITION_TIME_STEP = args.transit_time_step
PER_CONF_STEP = args.per_conf_step
# transition motion planner settings
RRT_RESTARTS = 5
RRT_ITERATIONS = 40
# choreo pkg settings
choreo_problem_instance_dir = compas_fab.get('choreo_instances')
unit_geos, static_obstacles = load_assembly_package(
choreo_problem_instance_dir, args.problem, scale=args.model_scale)
result_save_path = os.path.join(
choreo_problem_instance_dir, 'results',
'choreo_result.json') if args.save_result else None
# urdf, end effector settings
if args.robot == 'ur3':
# urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur3.urdf')
urdf_filename = compas_fab.get(
'universal_robot/ur_description/urdf/ur3_collision_viz.urdf')
srdf_filename = compas_fab.get(
'universal_robot/ur3_moveit_config/config/ur3.srdf')
else:
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')
# ee_sep_filename = compas_fab.get('universal_robot/ur_description/meshes/' +
# 'pychoreo_workshop_gripper/collision/victor_gripper_jaw03_rough_sep.obj')
# ee_decomp_file_dir = compas_fab.get('universal_robot/ur_description/meshes/' +
# 'pychoreo_workshop_gripper/collision/decomp')
# ee_decomp_file_prefix = 'victor_gripper_jaw03_decomp_'
# decomp_parts_num = 36
client = RosClient() if use_moveit_planner else None
# geometry file is not loaded here
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
base_link_name = robot.get_base_link_name()
ee_link_name = robot.get_end_effector_link_name()
ik_joint_names = robot.get_configurable_joint_names()
# parse end effector mesh
# ee_meshes = [Mesh.from_obj(os.path.join(ee_decomp_file_dir, ee_decomp_file_prefix + str(i) + '.obj')) for i in range(decomp_parts_num)]
ee_meshes = [Mesh.from_obj(ee_filename)]
# ee_meshes = [Mesh.from_obj(ee_sep_filename)]
# define TCP transformation
tcp_tf = Translation([0.099, 0, 0]) # in meters
ur5_start_conf = [0, -1.65715, 1.71108, -1.62348, 0, 0]
if use_moveit_planner:
# TODO: attach end effector to the robot in planning scene
# https://github.com/compas-dev/compas_fab/issues/66
scene = PlanningScene(robot)
scene.remove_all_collision_objects()
client.set_joint_positions(group, ik_joint_names, ur5_start_conf)
else:
scene = None
# add static collision obstacles
co_dict = {}
for i, static_obs_mesh in enumerate(static_obstacles):
# offset the table a bit...
cm = CollisionMesh(static_obs_mesh,
'so_' + str(i),
frame=Frame.from_transformation(
Translation([0, 0, -0.02])))
if use_moveit_planner:
scene.add_collision_mesh(cm)
else:
co_dict[cm.id] = {}
co_dict[cm.id]['meshes'] = [cm.mesh]
co_dict[cm.id]['mesh_poses'] = [cm.frame]
if use_moveit_planner:
# See: https://github.com/compas-dev/compas_fab/issues/63#issuecomment-519525879
time.sleep(1)
co_dict = scene.get_collision_meshes_and_poses()
# ======================================================
# ======================================================
# start pybullet environment & load pybullet robot
connect(use_gui=VIZ)
pb_robot = create_pb_robot_from_ros_urdf(urdf_filename,
urdf_pkg_name,
planning_scene=scene,
ee_link_name=ee_link_name)
ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot,
ee_link_name)
# update current joint conf and attach end effector
pb_ik_joints = joints_from_names(pb_robot, ik_joint_names)
pb_end_effector_link = link_from_name(pb_robot, ee_link_name)
if not use_moveit_planner:
set_joint_positions(pb_robot, pb_ik_joints, ur5_start_conf)
for e_at in ee_attachs:
e_at.assign()
# draw TCP frame in pybullet
if has_gui():
TCP_pb_pose = get_TCP_pose(pb_robot,
ee_link_name,
tcp_tf,
return_pb_pose=True)
handles = draw_pose(TCP_pb_pose, length=0.04)
# wait_for_user()
# deliver ros collision meshes to pybullet
static_obstacles_from_name = convert_meshes_and_poses_to_pybullet_bodies(
co_dict)
# for now...
for so_key, so_val in static_obstacles_from_name.items():
static_obstacles_from_name[so_key] = so_val[0]
for unit_name, unit_geo in unit_geos.items():
geo_bodies = []
for sub_id, mesh in enumerate(unit_geo.mesh):
geo_bodies.append(convert_mesh_to_pybullet_body(mesh))
unit_geo.pybullet_bodies = geo_bodies
# check collision between obstacles and element geometries
assert not sanity_check_collisions(unit_geos, static_obstacles_from_name)
# from random import shuffle
seq_assignment = list(range(len(unit_geos)))
# shuffle(seq_assignment)
element_seq = {seq_id: e_id for seq_id, e_id in enumerate(seq_assignment)}
# for key, val in element_seq.items():
# # element_seq[key] = 'e_' + str(val)
# element_seq[key] = val
if has_gui():
for e_id in element_seq.values():
# for e_body in brick_from_index[e_id].body: set_pose(e_body, brick_from_index[e_id].goal_pose)
handles.extend(
draw_pose(unit_geos[e_id].initial_pb_pose, length=0.02))
handles.extend(draw_pose(unit_geos[e_id].goal_pb_pose,
length=0.02))
for e_body in unit_geos[e_id].pybullet_bodies:
set_pose(e_body, unit_geos[e_id].initial_pb_pose)
print('pybullet env loaded.')
# wait_for_user()
for h in handles:
remove_debug(h)
saved_world = WorldSaver()
ik_fn = ikfast_ur3.get_ik if args.robot == 'ur3' else ikfast_ur5.get_ik
tot_traj, graph_sizes = \
direct_ladder_graph_solve_picknplace(pb_robot, ik_joint_names, base_link_name, ee_link_name, ik_fn,
unit_geos, element_seq, static_obstacles_from_name,
tcp_transf=pb_pose_from_Transformation(tcp_tf),
ee_attachs=ee_attachs,
max_attempts=100, viz=VIZ_IKFAST, st_conf=ur5_start_conf)
picknplace_cart_plans = divide_nested_list_chunks(tot_traj, graph_sizes)
saved_world.restore()
print('Cartesian planning finished.')
# reset robot and parts for better visualization
set_joint_positions(pb_robot, pb_ik_joints, ur5_start_conf)
for ee in ee_attachs:
ee.assign()
for e_id in element_seq.values():
for e_body in unit_geos[e_id].pybullet_bodies:
set_pose(e_body, unit_geos[e_id].initial_pb_pose)
# if has_gui():
# wait_for_user()
def flatten_unit_geos_bodies(in_dict):
out_list = []
for ug in in_dict.values():
out_list.extend(ug.pybullet_bodies)
return out_list
if plan_transition:
print('Transition planning started.')
for seq_id, unit_picknplace in enumerate(picknplace_cart_plans):
print('----\ntransition seq#{}'.format(seq_id))
e_id = element_seq[seq_id]
if seq_id != 0:
tr_start_conf = picknplace_cart_plans[seq_id -
1]['place_retreat'][-1]
else:
tr_start_conf = ur5_start_conf
# obstacles=static_obstacles + cur_mo_list
place2pick_st_conf = list(tr_start_conf)
place2pick_goal_conf = list(
picknplace_cart_plans[seq_id]['pick_approach'][0])
# assert not client.is_joint_state_colliding(group, ik_joint_names, place2pick_st_conf)
# assert not client.is_joint_state_colliding(group, ik_joint_names, place2pick_goal_conf)
if use_moveit_planner:
# TODO: add collision objects
st_conf = Configuration.from_revolute_values(
place2pick_st_conf)
goal_conf = Configuration.from_revolute_values(
place2pick_goal_conf)
goal_constraints = robot.constraints_from_configuration(
goal_conf, [math.radians(1)] * 6, group)
place2pick_jt_traj = robot.plan_motion(goal_constraints,
st_conf,
group,
planner_id='RRTConnect')
place2pick_path = [
jt_pt['values']
for jt_pt in place2pick_jt_traj.to_data()['points']
]
else:
saved_world = WorldSaver()
set_joint_positions(pb_robot, pb_ik_joints, place2pick_st_conf)
for ee_a in ee_attachs:
ee_a.assign()
place2pick_path = plan_joint_motion(
pb_robot,
pb_ik_joints,
place2pick_goal_conf,
attachments=ee_attachs,
obstacles=list(static_obstacles_from_name.values()) +
flatten_unit_geos_bodies(unit_geos),
self_collisions=True,
resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints),
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
)
saved_world.restore()
if not place2pick_path:
saved_world = WorldSaver()
print('****\nseq #{} cannot find place2pick transition'.
format(seq_id))
print('Diagnosis...')
cfn = get_collision_fn_diagnosis(pb_robot, pb_ik_joints, \
obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos),
attachments=ee_attachs, self_collisions=True)
print('start pose:')
cfn(place2pick_st_conf)
print('end pose:')
cfn(place2pick_goal_conf)
saved_world.restore()
print('Diagnosis over')
pick2place_st_conf = picknplace_cart_plans[seq_id]['pick_retreat'][
-1]
pick2place_goal_conf = picknplace_cart_plans[seq_id][
'place_approach'][0]
if use_moveit_planner:
st_conf = Configuration.from_revolute_values(
picknplace_cart_plans[seq_id]['pick_retreat'][-1])
goal_conf = Configuration.from_revolute_values(
picknplace_cart_plans[seq_id]['place_approach'][0])
goal_constraints = robot.constraints_from_configuration(
goal_conf, [math.radians(1)] * 6, group)
pick2place_jt_traj = robot.plan_motion(goal_constraints,
st_conf,
group,
planner_id='RRTConnect')
pick2place_path = [
jt_pt['values']
for jt_pt in pick2place_jt_traj.to_data()['points']
]
else:
saved_world = WorldSaver()
# create attachement without needing to keep track of grasp...
set_joint_positions(
pb_robot, pb_ik_joints,
picknplace_cart_plans[seq_id]['pick_retreat'][0])
# attachs = [Attachment(robot, tool_link, invert(grasp.attach), e_body) for e_body in brick.body]
element_attachs = [create_attachment(pb_robot, pb_end_effector_link, e_body) \
for e_body in unit_geos[e_id].pybullet_bodies]
set_joint_positions(pb_robot, pb_ik_joints, pick2place_st_conf)
for ee_a in ee_attachs:
ee_a.assign()
for e_a in element_attachs:
e_a.assign()
pick2place_path = plan_joint_motion(
pb_robot,
pb_ik_joints,
pick2place_goal_conf,
obstacles=list(static_obstacles_from_name.values()) +
flatten_unit_geos_bodies(unit_geos),
attachments=ee_attachs + element_attachs,
self_collisions=True,
resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints),
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
)
saved_world.restore()
if not pick2place_path:
saved_world = WorldSaver()
print('****\nseq #{} cannot find pick2place transition'.
format(seq_id))
print('Diagnosis...')
cfn = get_collision_fn_diagnosis(pb_robot, pb_ik_joints,
obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), \
attachments=ee_attachs + element_attachs, self_collisions=True)
print('start pose:')
cfn(pick2place_st_conf)
print('end pose:')
cfn(pick2place_goal_conf)
saved_world.restore()
print('Diagnosis over')
picknplace_cart_plans[seq_id]['place2pick'] = place2pick_path
picknplace_cart_plans[seq_id]['pick2place'] = pick2place_path
for e_body in unit_geos[e_id].pybullet_bodies:
set_pose(e_body, unit_geos[e_id].goal_pb_pose)
if seq_id == len(picknplace_cart_plans) - 1:
saved_world = WorldSaver()
set_joint_positions(
pb_robot, pb_ik_joints,
picknplace_cart_plans[seq_id]['place_retreat'][-1])
for ee_a in ee_attachs:
ee_a.assign()
return2idle_path = plan_joint_motion(
pb_robot,
pb_ik_joints,
ur5_start_conf,
obstacles=list(static_obstacles_from_name.values()) +
flatten_unit_geos_bodies(unit_geos),
attachments=ee_attachs,
self_collisions=True,
resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints),
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
)
saved_world.restore()
picknplace_cart_plans[seq_id]['return2idle'] = return2idle_path
print('Transition planning finished.')
# convert to ros JointTrajectory
traj_json_data = []
traj_time_count = 0.0
for i, element_process in enumerate(picknplace_cart_plans):
e_proc_data = {}
for sub_proc_name, sub_process in element_process.items():
sub_process_jt_traj_list = []
for jt_sol in sub_process:
sub_process_jt_traj_list.append(
JointTrajectoryPoint(values=jt_sol,
types=[0] * 6,
time_from_start=Duration(
traj_time_count, 0)))
traj_time_count += 1.0 # meaningless timestamp
e_proc_data[sub_proc_name] = JointTrajectory(
trajectory_points=sub_process_jt_traj_list,
start_configuration=sub_process_jt_traj_list[0]).to_data()
traj_json_data.append(e_proc_data)
if result_save_path:
with open(result_save_path, 'w+') as outfile:
json.dump(traj_json_data, outfile, indent=4)
print('planned trajectories saved to {}'.format(result_save_path))
print('\n*************************\nplanning completed. Simulate?')
if has_gui():
wait_for_user()
for e_id in element_seq.values():
for e_body in unit_geos[e_id].pybullet_bodies:
set_pose(e_body, unit_geos[e_id].initial_pb_pose)
display_picknplace_trajectories(pb_robot, ik_joint_names, ee_link_name,
unit_geos, traj_json_data, \
ee_attachs=ee_attachs,
cartesian_time_step=CART_TIME_STEP, transition_time_step=TRANSITION_TIME_STEP, step_sim=True, per_conf_step=PER_CONF_STEP)
if use_moveit_planner: scene.remove_all_collision_objects()
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)
def inverse_kinematics(self,
frame_WCF,
start_configuration=None,
group=None,
avoid_collisions=True,
constraints=None,
attempts=8,
attached_collision_meshes=None):
"""Calculate the robot's inverse kinematic for a given frame.
Parameters
----------
frame: :class:`compas.geometry.Frame`
The frame to calculate the inverse for.
start_configuration: :class:`compas_fab.robots.Configuration`, optional
If passed, the inverse will be calculated such that the calculated
joint positions differ the least from the start_configuration.
Defaults to the init configuration.
group: str, optional
The planning group used for calculation. Defaults to the robot's
main planning group.
avoid_collisions: bool, optional
Whether or not to avoid collisions. Defaults to True.
constraints: list of :class:`compas_fab.robots.Constraint`, optional
A set of constraints that the request must obey. Defaults to None.
attempts: int, optional
The maximum number of inverse kinematic attempts. Defaults to 8.
attached_collision_meshes: list of :class:`compas_fab.robots.AttachedCollisionMesh`
Defaults to None.
Raises
------
compas_fab.backends.exceptions.BackendError
If no configuration can be found.
Returns
-------
:class:`compas_fab.robots.Configuration`
The planning group's configuration.
Examples
--------
>>> frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
>>> start_configuration = robot.init_configuration()
>>> group = robot.main_group_name
>>> configuration = robot.inverse_kinematics(frame_WCF, start_configuration, group)
"""
self.ensure_client()
if not group:
group = self.main_group_name # ensure semantics
base_link = self.get_base_link_name(group)
joint_names = self.get_configurable_joint_names()
joint_positions = self._get_scaled_joint_positions_from_start_configuration(
start_configuration)
# represent in RCF
frame_RCF = self.represent_frame_in_RCF(frame_WCF, group)
frame_RCF.point /= self.scale_factor # must be in meters
response = self.client.inverse_kinematics(frame_RCF, base_link, group,
joint_names, joint_positions,
avoid_collisions,
constraints, attempts,
attached_collision_meshes)
joint_positions = response.solution.joint_state.position
joint_positions = self._scale_joint_values(joint_positions,
self.scale_factor)
# full configuration # TODO group config?
configuration = Configuration(joint_positions,
self.get_configurable_joint_types())
return configuration
def init_configuration(self, group=None):
"""Returns the init joint configuration.
"""
types = [joint.type for joint in self.get_configurable_joints(group)]
positions = [0.] * len(types)
return Configuration(positions, types)