c_y = math.sin(counter)
s = 0.1
num_ee = relaxedIK.vars.robot.numChains
goal_pos = []
goal_quat = []
goal_pos.append([s * c_x + 0.327, s * c_y, 0])
for i in range(num_ee):
goal_quat.append([1, 0, 0, 0])
if i == 0:
continue
else:
goal_pos.append([0, 0, 0])
xopt = relaxedIK.solve(goal_pos, goal_quat)
js = joint_state_define(xopt)
if js == None:
js = JointState()
js.name = joint_ordering
for x in xopt:
js.position.append(x)
now = rospy.Time.now()
js.header.stamp.secs = now.secs
js.header.stamp.nsecs = now.nsecs
js_pub.publish(js)
tf_pub.sendTransform((0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), 'common_world', fixed_frame)
counter += stride
pre_config=True)
sample_states = vars.collision_graph.sample_states
idx = 0
rate = rospy.Rate(0.8)
while not rospy.is_shutdown():
if idx >= len(sample_states): idx = 0
state = sample_states[idx]
cg = vars.collision_graph
robot = vars.robot
frames = robot.getFrames(state)
cg.get_collision_score(frames)
cg.c.draw_all()
js = joint_state_define(state)
if js == None:
js = JointState()
js.name = joint_ordering
for x in state:
js.position.append(x)
now = rospy.Time.now()
js.header.stamp.secs = now.secs
js.header.stamp.nsecs = now.nsecs
js_pub.publish(js)
tf_pub.sendTransform((0, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), 'common_world', fixed_frame)
marker = Marker()