def FollowTraj(self, traj):
q_traj = [p.positions for p in traj.points]
dq_traj = [p.velocities for p in traj.points]
t_traj = [p.time_from_start for p in traj.points]
#If no initial point:
if t_traj[0].to_sec() > 1.0e-3:
q_traj = [self.js.position] + q_traj
dq_traj = [self.js.velocity] + dq_traj
t_traj = [rospy.Duration(0.0)] + t_traj
print 'Received trajectory command:'
print[t.to_sec() for t in t_traj]
print q_traj
#Modeling the trajectory with spline.
splines = [TCubicHermiteSpline() for d in range(7)]
for d in range(len(splines)):
data_d = [[t.to_sec(), q[d]] for q, t in zip(q_traj, t_traj)]
splines[d].Initialize(data_d,
tan_method=splines[d].CARDINAL,
c=0.0,
m=0.0)
rate = rospy.Rate(self.rate)
t0 = rospy.Time.now()
while all(((rospy.Time.now() - t0) < t_traj[-1],
self.follow_traj_active, not rospy.is_shutdown())):
t = (rospy.Time.now() - t0).to_sec()
q = [splines[d].Evaluate(t) for d in range(7)]
#print t, q
with self.js_locker:
self.js.position = copy.deepcopy(q)
rate.sleep()
def InterpolateTraj(q_traj, t_traj, dt=1.0e-3):
if t_traj[0]>1.0e-6:
raise Exception('t_traj[0] must be zero and q_traj[0] must be the current joint angles.')
assert(len(q_traj)==len(t_traj))
#Modeling the trajectory with spline.
DoF= len(q_traj[0])
splines= [TCubicHermiteSpline() for d in range(DoF)]
for d in range(DoF):
data_d= [[t,q[d]] for q,t in zip(q_traj,t_traj)]
splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0)
traj_points= []
t= 0.0
while t<t_traj[-1]:
q,v,a= [],[],[]
for spline in splines:
q_d,v_d,a_d= spline.Evaluate(t,with_dd=True)
q.append(q_d)
v.append(v_d)
a.append(a_d)
point= trajectory_msgs.msg.JointTrajectoryPoint()
point.positions= q
point.velocities= v
point.accelerations= a
point.time_from_start= rospy.Duration(t)
traj_points.append(point)
t+= dt
#JTP= trajectory_msgs.msg.JointTrajectoryPoint
#traj_points= np.array([JTP(*(np.array([[spline.Evaluate(t,with_dd=True)] for spline in splines]).T.tolist()+[[],rospy.Duration(t)]))
#for t in np.arange(0.0,t_traj[-1],dt)])
return traj_points
def TrajectoryController(self, joint_names, q_traj, t_traj, current,
callback):
assert (len(t_traj) > 0)
assert (len(q_traj) == len(t_traj))
assert (len(joint_names) == len(q_traj[0]))
dof = len(joint_names)
#Revising trajectory (add an initial point).
if t_traj[0] > 1.0e-3:
q_traj = [self.Position(joint_names)] + q_traj
t_traj = [0.0] + t_traj
#Modeling the trajectory with spline.
splines = [TCubicHermiteSpline() for d in xrange(dof)]
for d in xrange(len(splines)):
data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)]
splines[d].Initialize(data_d,
tan_method=splines[d].CARDINAL,
c=0.0,
m=0.0)
rate = TRate(self.hz_traj_ctrl)
t0 = time.time()
while all(((time.time() - t0) < t_traj[-1],
self.threads['TrajectoryController'][0])):
t = time.time() - t0
#q= [splines[d].Evaluate(t) for d in xrange(dof)]
q_dq = [splines[d].Evaluate(t, with_tan=True) for d in range(dof)]
q = [q for q, _ in q_dq]
dq = [dq for _, dq in q_dq]
if callback is not None:
if callback('loop_begin', t, q, dq) == False: break
#print t, q
if current is None:
with self.port_locker:
self.MoveTo(
{jname: qj
for jname, qj in zip(joint_names, q)},
blocking=False)
else:
with self.port_locker:
self.MoveToC(
{
jname: (qj, ej)
for jname, qj, ej in zip(joint_names, q, current)
},
blocking=False)
if callback is not None:
callback('loop_end', None, None, None)
rate.sleep()
self.threads['TrajectoryController'][0] = False
if callback is not None:
callback('final', None, None, None)
def QTrajToDQTraj(self,q_traj, t_traj): dof= len(q_traj[0]) #Modeling the trajectory with spline. splines= [TCubicHermiteSpline() for d in range(dof)] for d in range(len(splines)): data_d= [[t,q[d]] for q,t in zip(q_traj,t_traj)] splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0) #NOTE: We don't have to make spline models as we just want velocities at key points. # They can be obtained by computing tan_method, which will be more efficient. with_tan=True dq_traj= [] for t in t_traj: dq= [splines[d].Evaluate(t,with_tan=True)[1] for d in range(dof)] dq_traj.append(dq) #print dq_traj return dq_traj
def Main():
from cubic_hermite_spline import TCubicHermiteSpline
data= GenNd_1(x0=[0.0]*Nd, xf=[1.0]*Nd, param=[RandN([-1.0,-1.0],[1.0,1.0]) for d in range(Nd)])
splines= [TCubicHermiteSpline() for d in range(len(data[0])-1)]
for d in range(len(splines)):
data_d= [[x[0],x[d+1]] for x in data]
splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0)
pf= open('/tmp/spline1.dat','w')
t= data[0][0]
while True:
x= [splines[d].Evaluate(t) for d in range(len(splines))]
pf.write('%f %s\n' % (t, ' '.join(map(str,x))))
if t>data[-1][0]: break
t+= 0.02
#t+= 0.001
print 'Generated:','/tmp/spline1.dat'
pf= open('/tmp/spline0.dat','w')
for d in data:
pf.write('%s\n' % ' '.join(map(str,d)))
print 'Generated:','/tmp/spline0.dat'
if __name__ == '__main__':
def PrintEq(s):
print '%s= %r' % (s, eval(s))
from cubic_hermite_spline import TCubicHermiteSpline
from vel_ctrl import ModifyTrajVelocityV
import gen_data
import math
#data= gen_data.Gen3d_1()
data = gen_data.Gen3d_2()
#data= gen_data.Gen3d_3()
#data= gen_data.Gen3d_4()
splines = [TCubicHermiteSpline() for d in range(len(data[0]) - 1)]
for d in range(len(splines)):
data_d = [[x[0], x[d + 1]] for x in data]
splines[d].Initialize(data_d,
tan_method=splines[d].CARDINAL,
c=0.0,
m=0.0)
dt = 0.005
pf = file('/tmp/spline1.dat', 'w')
t = data[0][0]
while True:
x = [splines[d].Evaluate(t) for d in range(len(splines))]
pf.write('%f %s\n' % (t, ' '.join(map(str, x))))
if t > data[-1][0]: break
t += dt
#!/usr/bin/python
if __name__ == "__main__":
from cubic_hermite_spline import TCubicHermiteSpline
import gen_data
spline = TCubicHermiteSpline()
#data= gen_data.Gen1d_1()
#data= gen_data.Gen1d_2()
#data= gen_data.Gen1d_3()
data = gen_data.Gen1d_cyc1()
#data= gen_data.Gen1d_cyc2()
#data= gen_data.Gen1d_cyc3()
#FINITE_DIFF, CARDINAL
spline.Initialize(data,
tan_method=spline.FINITE_DIFF,
end_tan=spline.CYCLIC,
c=0.0)
#spline.KeyPts[0].M= 5.0
#spline.KeyPts[-1].M= -5.0
pf = file('/tmp/spline0.dat', 'w')
for d in data:
pf.write('%f %f\n' % (d[0], d[1]))
print 'Generated:', '/tmp/spline0.dat'
pf = file('/tmp/spline1.dat', 'w')
t = -5.0
while t < 5.0:
def QTrajToDQTraj(self,q_traj, t_traj):
dof= len(q_traj[0])
#Modeling the trajectory with spline.
splines= [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d= [[t,q[d]] for q,t in zip(q_traj,t_traj)]
splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj= []
for t in t_traj:
dq= [splines[d].Evaluate(t,with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
print dq_traj
return dq_traj
def JointNames(self):
#arm= 0
return self.joint_names[0]
def ROSGetJTP(self,q,t,dq=None):
jp= trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions= q
jp.time_from_start= rospy.Duration(t)
if dq is not None: jp.velocities= dq
return jp
def ToROSTrajectory(self,joint_names, q_traj, t_traj, dq_traj=None):
assert(len(q_traj)==len(t_traj))
if dq_traj is not None: (len(dq_traj)==len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names= joint_names
if dq_traj is not None:
self.traj.points= [self.ROSGetJTP(q,t,dq) for q,t,dq in zip(q_traj, t_traj, dq_traj)]
else:
self.traj.points= [self.ROSGetJTP(q,t) for q,t in zip(q_traj, t_traj)]
self.traj.header.stamp= rospy.Time.now()
print self.traj
return self.traj
def SmoothQTraj(self,q_traj):
if len(q_traj)==0: return
q_prev= np.array(q_traj[0])
q_offset= np.array([0]*len(q_prev))
for q in q_traj:
q_diff= np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d]<-math.pi: q_offset[d]+=1
elif q_diff[d]>math.pi: q_offset[d]-=1
q_prev= copy.deepcopy(q)
q[:]= q+q_offset*2.0*math.pi
def setupSence(self):
r_tool_size=[0.03,0.02,0.18]
l_tool_size=[0.03,0.02,0.18]
'''
#sim table
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=0.75
table_pose.pose.position.y=0.0
table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0
table_pose.pose.orientation.w=1.0
self.scene.add_box(self.table_id,table_pose,self.table_size)
'''
#real scene table
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=-0.184
table_pose.pose.position.y=0.62
table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0
table_pose.pose.orientation.w=1.0
self.scene.add_box(self.table_id,table_pose,self.table_size)
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=self.arm_end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.057
l_p.pose.position.z=0.09
l_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,self.l_id,l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=self.arm_end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y=-0.057
r_p.pose.position.z=0.09
r_p.pose.orientation.w=1
self.scene.attach_box(self.arm_end_effector_link,self.r_id,r_p,r_tool_size)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene=PlanningSceneInterface()
pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=1)
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition)
self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
cartesian=rospy.get_param('~cartesian',True)
self.arm_end_effector_link=arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
self.reference_frame='base_link'
arm.set_pose_reference_frame(self.reference_frame)
arm.set_planning_time(5)
#scene planning
self.l_id='l_tool'
self.r_id='r_tool'
self.table_id='table'
self.target_id='target_object'
self.f_target_id='receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.scene.remove_world_object(self.f_target_id)
self.table_ground=0.13
self.table_size=[0.9,0.6,0.018]
self.setupSence()
joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')]
joint_names= rospy.get_param('controller_joint_names')
traj= trajectory_msgs.msg.JointTrajectory()
traj.joint_names= joint_names
target_size=[0.058,0.058,0.19]
f_target_size=[0.2,0.2,0.04]
#final target
f_target_pose=PoseStamped()
f_target_pose.header.frame_id=self.reference_frame
f_target_pose.pose.position.x=-0.184+0.27
f_target_pose.pose.position.y=0.62+0.1
f_target_pose.pose.position.z=self.table_ground+self.table_size[2]+f_target_size[2]/2.0
f_target_pose.pose.orientation.x=0
f_target_pose.pose.orientation.y=0
f_target_pose.pose.orientation.z=0
f_target_pose.pose.orientation.w=1
self.scene.add_box(self.f_target_id,f_target_pose,f_target_size)
#pouring pose
pour_pose=f_target_pose
pour_pose.pose.position.x-=0.06
pour_pose.pose.position.y-=0.12
pour_pose.pose.position.z+=0.15
#pour_pose.pose.position.y+=0.17
pour_pose.pose.orientation.x=-0.5
pour_pose.pose.orientation.y=-0.5
pour_pose.pose.orientation.z=-0.5
pour_pose.pose.orientation.w=0.5
#set color
self.setColor(self.table_id,0.8,0,0,1.0)
self.setColor(self.f_target_id,0.8,0.4,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor(self.target_id,0,1,0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(5)
#j_ori_state=[0.72316974401474, 0.4691084027290344, 0.41043856739997864, -2.3381359577178955, 0.5580500364303589, 1.1085972785949707, 3.14]
j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_trans_state=[1.708616018295288, 0.9996442198753357, -0.4782222807407379, -1.7541601657867432, 0.13480804860591888, 1.1835496425628662, 2.689549684524536]
signal= True
arm.set_joint_value_target(j_ori_state)
arm.go()
rospy.sleep(3)
#arm.set_joint_value_target(j_trans_state)
#arm.go()
#rospy.sleep(5)
waypoints_1=[]
waypoints_2=[]
start_pick_pose=arm.get_current_pose(self.arm_end_effector_link).pose
if cartesian:
msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
target_pose=PoseStamped()
target_pose.header.frame_id=self.reference_frame
target_pose.pose.position.x=-(msg.pose.position.y)-0.28
target_pose.pose.position.y=-msg.pose.position.x+0.81-0.072
target_pose.pose.position.z=1.36-msg.pose.position.z+0.1
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=-0
target_pose.pose.orientation.w=1
self.scene.add_box(self.target_id,target_pose,target_size)
self.setColor(self.target_id,0,0.8,0)
self.sendColors()
#pre_g_pose
pre_grasp_pose=PoseStamped()
pre_grasp_pose.header.frame_id=self.reference_frame
pre_grasp_pose.pose.position=target_pose.pose.position
pre_grasp_pose.pose.position.y-=0.2
pre_grasp_pose.pose.position.z+=0.01
pre_grasp_pose.pose.orientation.x=-0.5
pre_grasp_pose.pose.orientation.y=-0.5
pre_grasp_pose.pose.orientation.z=-0.5
pre_grasp_pose.pose.orientation.w=0.5
#grasp_pose
grasp_pose=PoseStamped()
grasp_pose.header.frame_id=self.reference_frame
grasp_pose.pose.position=target_pose.pose.position
grasp_pose.pose.position.y+=0.025
grasp_pose.pose.orientation.x=-0.5
grasp_pose.pose.orientation.y=-0.5
grasp_pose.pose.orientation.z=-0.5
grasp_pose.pose.orientation.w=0.5
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y= -0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
waypoints_1.append(start_pick_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_1.append(deepcopy(grasp_pose.pose))
fraction=0.0
attempts=0
maxtries=300
while fraction!=1 and attempts<maxtries:
(plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True)
attempts+=1
if (attempts %300==0 and fraction!=1):
rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.")
signal_pick=False
if fraction==1:
rospy.loginfo("path compute successfully with "+str(attempts)+" sttempts.")
rospy.loginfo("Arm moving.")
rospy.sleep(3)
#arm.execute(plan_1)
p_pick_1=plan_1.joint_trajectory.points[-1]
#print p
arm.set_joint_value_target(p_pick_1.positions)
arm.go()
rospy.sleep(7)
self.gripperCtrl(0)
rospy.sleep(3)
self.scene.remove_world_object(self.target_id)
#p_pick_2=plan_1.joint_trajectory.points[1]
#print p
#arm.set_joint_value_target(p_pick_2.positions)
#arm.go()
#rospy.sleep(4)
signal_pick=True
#p=plan_1.joint_trajectory.points[-1]
#print p
#arm.set_joint_value_target(p.positions)
#arm.go()
if signal_pick:
start_trans_pose=arm.get_current_pose(self.arm_end_effector_link).pose
waypoints_2.append(start_trans_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_2.append(deepcopy(pour_pose.pose))
fraction_2=0.0
attempts_2=0
maxtries_2=300
while fraction_2!=1 and attempts_2<maxtries_2:
(plan_2,fraction_2)=arm.compute_cartesian_path(waypoints_2,0.01,0.0,True)
attempts_2+=1
if (attempts_2 %300==0 and fraction_2!=1):
rospy.loginfo("path planning failed with " + str(fraction_2*100)+"% success.")
if fraction_2==1:
#signal=False
rospy.loginfo("path compute successfully with "+str(attempts_2)+" sttempts.")
rospy.loginfo("Arm moving.")
p_trans_shake=plan_2.joint_trajectory.points[-1]
p_trans=plan_2.joint_trajectory.points[-2]
#print p
arm.set_joint_value_target(p_trans.positions)
arm.go()
rospy.sleep(3)
j_pre_pour_state=arm.get_current_joint_values()
j_pour_state=j_pre_pour_state
j_pour_state[6]-=(1.57+0.26)
arm.set_joint_value_target(j_pour_state)
arm.go()
print "shaking balabala"
rospy.sleep(1)
print "shaking balabala"
rospy.sleep(1)
print "shaking balabala"
rospy.sleep(1)
#shake=True
for i in range(10):
self.add_point(traj, 0.3, p_trans_shake, [0.0]*7)
self.add_point(traj, 0.3, p_trans, [0.0]*7)
traj.header.stamp= rospy.Time.now()
pub_traj.publish(traj)
arm.set_joint_value_target(j_pre_pour_state)
arm.go()
rospy.sleep(6)
arm.set_joint_value_target(p_pick_1.positions)
arm.go()
rospy.sleep(7)
self.gripperCtrl(255)
#remove and shut down
#self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
#self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
diff = lambda x2, x1: [x2d - x1d for (x2d, x1d) in zip(x2, x1)]
norm = lambda x2, x1: la.norm(diff(x2, x1))
is_samedir = lambda s1, s2: np.dot(s1, s2) > 0
return modify_traj_velocity_base(t0, v, traj, time_step, T, num_iter_l,
num_iter_e, norm, diff, is_samedir)
if __name__ == '__main__':
def PrintEq(s):
print '%s= %r' % (s, eval(s))
from cubic_hermite_spline import TCubicHermiteSpline
import gen_data
import math
spline = TCubicHermiteSpline()
data = gen_data.Gen1d_1()
#data= gen_data.Gen1d_2()
#data= gen_data.Gen1d_3()
spline.Initialize(data, tan_method=spline.CARDINAL, c=0.0)
dt = 0.005
pf = file('/tmp/spline1.dat', 'w')
t = data[0][0]
while t < data[-1][0]:
pf.write('%f %f\n' % (t, spline.Evaluate(t)))
t += dt
print 'Generated:', '/tmp/spline1.dat'