Commit d545b738 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

execute plan for single arm in moveIt, support simulation in controller (implement #11)

parent 84a1799f
......@@ -59,7 +59,8 @@ class Controller(object):
extruderTemp_C=190.0,
hotend_srv='hotend',
retractionDist_mm=12.0,
retractionSpeed_mmpmin=4800.0):
retractionSpeed_mmpmin=4800.0,
simulation=False):
"""
Creates a controller to multiple robot arms. (TODO)
......@@ -77,7 +78,8 @@ class Controller(object):
queue_size=1)
time.sleep(0.1)
rospy.wait_for_service(hotend_srv)
if not simulation:
rospy.wait_for_service(hotend_srv)
try:
self.extruderSetTemp_srv = rospy.ServiceProxy(hotend_srv,Hotend)
except rospy.ServiceException as e:
......@@ -100,9 +102,10 @@ class Controller(object):
self.extruderReady = False
# Start extruder
self.set_extruder_temp(extruderTemp_C)
if blocking:
self.wait_extruder_ready()
if not simulation:
self.set_extruder_temp(extruderTemp_C)
if blocking:
self.wait_extruder_ready()
## Arm methods
......
......@@ -29,6 +29,10 @@ def execute_plan(plan, controller):
Single arm only
"""
# plan to the start position of the plan
controller.group.go(plan.trajs[0][0].positions[0])
# execute trajectories in plan
for t in plan.trajs[0]:
jt = JTraj2ROS(t)
try:
......@@ -40,6 +44,6 @@ def execute_plan(plan, controller):
if __name__ == '__main__':
plan = load_plan('plans/1.plan')
con = Controller()
plan = load_plan('plans/3.plan')
con = Controller(simulation=True)
execute_plan(plan, con)
......@@ -51,11 +51,19 @@ def joints_at_time(traj, t):
return (t-t1)/(t2-t1)*(j2-j1) + j1
def JTraj2ROS(jtraj):
default_joint_names = ['shoulder_pan_joint',
'shoulder_lift_joint',
'elbow_joint',
'wrist_1_joint',
'wrist_2_joint',
'wrist_3_joint']
def JTraj2ROS(jtraj,joint_names=default_joint_names):
"""
convert JTrajectory object to ROS JointTrajectory
"""
j = JointTrajectory()
j.joint_names = joint_names
for i in range(len(jtraj.positions)):
jp = JointTrajectoryPoint()
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment