def set_initial_state_service(self, req):
self.set_initial_state()
return EmptyResponse()
self.raise_legs_on_frame(C.FRAME.EVEN_AND_ODD)
self.move_prismatic(-0.2)
self.actuators.wait_for_all_actuators_to_finish()
f.print_ros("Initial position COMPLETED")
def read(self):
efforts = rospy.get_param("robostilt/efforts")
self.leg_max = efforts["leg_max"]
self.lowering_leg_min = efforts["lowering_leg_min"]
self.lowering_leg_max = efforts["lowering_leg_max"]
f.print_ros("Parameters: efforts updated")
def move_actuator(self, actuator_index, position):
f.print_ros("Moving "+str(C.ACTUATOR.getNameFromIndex(actuator_index)
)+" to " + str(position) + "...")
i = actuator_index
self.actuators.actuator[i].motor.set_effort_limits_to_max()
self.actuators.actuator[i].motor.set_position(
position, p.speed.lowering_legs)
def update_robot_state_topic(self):
msg = RoboStiltStateMessage()
# Lower legs on ODD
f.wait_for_user()
self.lower_legs_on_frame(C.FRAME.ODD, True)
# Raise legs on even
f.wait_for_user()
# Move Forward
f.wait_for_user()
self.move_prismatic(-0.5)
# Lower legs on EVEN
f.wait_for_user()
self.lower_legs_on_frame(C.FRAME.EVEN, True)
# Raise legs on ODD
f.wait_for_user()
self.raise_legs_on_frame(C.FRAME.EVEN)
f.wait_for_user()
# service request is empty
f.print_ros(
"Initializing position. All legs raised prismatic to -0.2...")
f.wait_for_user()
self.raise_legs_on_frame(C.FRAME.EVEN_AND_ODD)
self.move_prismatic(-0.2)
self.actuators.wait_for_all_actuators_to_finish()
f.print_ros("Initial position COMPLETED")
def lower_legs_on_frame(self, frame, limited_effort):
f.print_ros("Lowering legs on frame " + frame.name + " to position " + str(
p.dimension.nominal_walking_height) + " with effort_limit= " + str(limited_effort) + " ...")
indexes=frame.actuatorIndexes
for i in indexes:
if(limited_effort == True):
self.actuators.actuator[i].motor.set_effort_limits(
p.effort.lowering_leg_min, p.effort.lowering_leg_max)
def move_actuator(self, actuator_index, position):
msg.header.stamp = rospy.Time.now()
msg.name = []
msg.is_supporting = []
for i in range(0, C.ACTUATOR.count):
self.actuators.actuator[i].motor.set_position(
msg.is_supporting.append(self.actuators.actuator[i].is_supporting)
self.actuators.wait_for_all_actuators_to_finish()
f.print_ros("Moving "+str(C.ACTUATOR.getNameFromIndex(actuator_index)
)+" to " + str(position) + "COMPLETED")
def read(self):
dimensions = rospy.get_param("robostilt/dimensions")
self.nominal_walking_height = dimensions["nominal_walking_height"]
f.print_ros("Parameters: dimensions updated")
def read(self):
speeds = rospy.get_param("robostilt/speeds")
self.lowering_legs = speeds["lowering_legs"]
self.raising_legs = speeds["raising_legs"]
self.prismatic = speeds["prismatic"]
f.print_ros("Parameters: speeds updated")
def read(self):
constraints = rospy.get_param("robostilt/constraints")
self.goal_pos = constraints["goal_pos"]
self.goal_time = constraints["goal_time"]
self.trajectory = constraints["trajectory"]
f.print_ros("Parameters: constraints updated")
def __init__(self):
f.print_ros("Robot_state setup started")
# start robot_state node and topic publisher
self.publisher = rospy.Publisher(
'/robostilt/general_state_topic', RoboStiltStateMessage, queue_size=1, latch=True)
'/robostilt/general_state_topic', RoboStiltStateMessage, queue_size = 1, latch = True)
def move_prismatic(self, position):
f.print_ros("Moving third frame Prismatic to " + str(position) + "...")
i = C.ACTUATOR.third_frame_prismatic.index
self.actuators.actuator[i].motor.set_effort_limits_to_max()
self.actuators.actuator[i].motor.set_position(
position, p.speed.prismatic)
if(frame.name == C.FRAME.EVEN.name):
lonely_leg=2
self.actuators.actuator[lonely_leg].motor.set_position(position, speed)
self.actuators.actuator[4].motor.set_position(-2.0, speed)
self.actuators.actuator[6].motor.set_position(-2.0, speed)
while(self.actuators.have_all_actuators_reached_goal() == False):
self.actuators.actuator[4].motor.set_position(-2.0, speed)
self.actuators.actuator[6].motor.set_position(-2.0, speed)
while(self.actuators.have_all_actuators_reached_goal() == False):
if(self.roll_x > level_thereshold):
self.actuators.actuator[6].motor.set_position(-2.0, speed*0.5)
f.print_ros("reducing speed")
elif(self.roll_x < -1*level_thereshold):
self.actuators.actuator[6].motor.set_position(-2.0, speed*1.5)
self.actuators.actuator[6].motor.set_position(-2.0, speed*0.5)
f.print_ros("reducing speed")
self.actuators.actuator[6].motor.set_position(-2.0, speed)
self.actuators.actuator[6].motor.set_position(-2.0, speed*1.5)
f.print_ros("Increasing speed")
# check if lonely leg is done
if (self.actuators.actuator[2].motor.has_reached_goal == True):
f.print_ros("normal speed")
# check if lonely leg is done
break
self.actuators.wait_for_all_actuators_to_finish()
#! /usr/bin/env python
import rospy
import robot_state
from constants import constants as C
from functions import print_ros
print_ros("Main starting...")
robostilt=robot_state.robot_state()
robostilt.set_initial_state()
#robostilt.lower_legs_on_frame(C.FRAME.EVEN,False)
robostilt.move_actuator(4,-0.2) #fall on purpose
robostilt.raise_frame_while_leveling(-1.0)
# while(True):
# robostilt.step_forward()
# #print_ros("Done with stepForward")
rospy.spin()
