def main():
radius = 0.05 # (meter)
center = [0.2, 0.15] # (x,z) meter
robotjoints = rospy.wait_for_message('/joint_states', sensor_msgs.msg.JointState)
q0 = robotjoints.position
rospy.sleep(1)
#for theta in np.linspace(0, 4*np.pi):
#for i in np.linspace(0,1,2):
#target_xz = [center[0] + radius * np.cos(theta) , center[1] + radius * np.sin(theta) ]
target_xz = [0.3, 0]
#target_xz = [-0.035,0.29]
q_sol = planner.ik(target_xz, q0)
if q_sol is None:
print 'no ik solution'
else:
print '(q_1,q_2)=', q_sol
if use_real_arm:
exec_joint1_pub.publish(std_msgs.msg.Float64(q_sol[0]))
exec_joint2_pub.publish(std_msgs.msg.Float64(q_sol[1]))
else:
js = sensor_msgs.msg.JointState(name=['joint1', 'joint2'], position = q_sol)
exec_joint_pub.publish(js)
q0 = q_sol
rospy.sleep(5)
print('The gripper may start moving');
gripper_msg = gripper()
gripper_msg.task = 5
gripper_pub.publish(gripper_msg);
print('The gripper command is working');
def solveIk(target_pose):
global q0
if q0 is None:
robotjoints = rospy.wait_for_message('/joint_states',
sensor_msgs.msg.JointState)
q0 = robotjoints.position[0:2]
q_sol = planner.ik([target_pose[0], target_pose[2]], q0)
if q_sol is None:
print 'no ik solution'
return
print '(q_1,q_2)=', q_sol
if use_real_arm:
exec_joint1_pub.publish(std_msgs.msg.Float64(q_sol[0]))
exec_joint2_pub.publish(std_msgs.msg.Float64(q_sol[1]))
else:
js = sensor_msgs.msg.JointState(name=['joint1', 'joint2'],
position=q_sol)
exec_joint_pub.publish(js)
q0 = q_sol
use_real_arm = rospy.get_param('/real_arm', False)
if __name__ == "__main__":
radius = 0.05 # (meter)
center = [0.3, 0.0] # (x,z) meter
robotjoints = rospy.wait_for_message('/joint_states',
sensor_msgs.msg.JointState)
q0 = robotjoints.position
for theta in np.linspace(0, 4 * np.pi, 500):
target_xz = (radius * np.cos(theta) + center[0],
radius * np.sin(theta) + center[1]
) ## [??, ??] use theta in your code
q_sol = planner.ik(target_xz, q0) ## planner.ik( ?? )
if q_sol is None:
print 'no ik solution'
else:
print '(q_1,q_2)=', q_sol
if use_real_arm:
exec_joint1_pub.publish(std_msgs.msg.Float64(q_sol[0]))
exec_joint2_pub.publish(std_msgs.msg.Float64(q_sol[1]))
else:
js = sensor_msgs.msg.JointState(name=['joint1', 'joint2'],
position=q_sol)
exec_joint_pub.publish(js)
q0 = q_sol
rospy.sleep(0.02)