# m.points[1] = endpoint
# elif m.id == 8: # from elbow2 to end
# m.points[0] = simulationMarkerArray.markers[4].points[1]
# m.points[1] = endpoint
# elif m.id == 9: # from elbow3 to end
# m.points[0] = simulationMarkerArray.markers[5].points[1]
# m.points[1] = endpoint
#
# return simulationMarkerArray
topic = "delta_simulation"
publisher = rospy.Publisher(topic, MarkerArray)
rospy.init_node("delta_robot")
markerArray = create_delta_simulation()
initializedMarkerArray = set_initial_delta_pose(markerArray)
try:
f = open("valid_points.txt", "a")
onlyone = True
while not rospy.is_shutdown():
theta = 1
idcount = 15
testpointscale = 0.002
while theta < 90 and onlyone:
print "Setting all dynamixel to " + str(theta)
print "Forward kinematics give us: "
posefinal = delta_calcForward(theta, theta, theta)
print posefinal
import roslib
roslib.load_manifest('delta_robot')
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import rospy
import math
from delta_construct import create_delta_simulation, set_initial_delta_pose
from delta_kinematics import *
import csv
topic = 'delta_simulation'
publisher = rospy.Publisher(topic, MarkerArray)
rospy.init_node('delta_robot')
markerArray = create_delta_simulation()
initializedMarkerArray = set_initial_delta_pose(markerArray)
idcount = 100
pointwidth = 0.005
with open('valid_points_generated.txt', 'rb') as f:
reader = csv.reader(f)
for row in reader:
if idcount < 50000:
if len(row) > 0:
testpoint = Marker()
testpoint.header.frame_id = "/map"
testpoint.type = testpoint.SPHERE
testpoint.action = testpoint.ADD