Commit 0d69ccc5 authored by Gabriele Buondonno's avatar Gabriele Buondonno

Remove optional feet position in DummyWalkingPatternGenerator

parent f3058532
......@@ -72,9 +72,6 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(footRight, MatrixHomogeneous);
DECLARE_SIGNAL_IN(waist, MatrixHomogeneous);
DECLARE_SIGNAL_IN(footPositionLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(footPositionRight, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(vcom, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(acom, dynamicgraph::Vector);
......
......@@ -41,23 +41,49 @@ robot.dynamic.WT.recompute(0)
# -------------------------- DESIRED TRAJECTORY --------------------------
# --- Trajectory generators
robot.comTrajGen = create_com_trajectory_generator(dt,robot)
robot.lfPosTrajGen = create_position_trajectory_generator(dt, robot, 'LF')
robot.rfPosTrajGen = create_position_trajectory_generator(dt, robot, 'RF')
# --- Walking pattern generator
# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
# robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
# robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
# --- Waist
robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, 'WT')
# robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
robot.waistMix = Mix_of_vector("waistMix")
robot.waistMix.setSignalNumber(3)
robot.waistMix.addSelec(1, 0, 3)
robot.waistMix.addSelec(2, 3, 3)
robot.waistMix.default.value = [0.0]*6
robot.waistMix.signal("sin1").value = [0.0]*3
plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m')
plug(robot.waistMix.sout, robot.waistToMatrix.sin)
# --- Interface with controller entities
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
wp.omega.value = omega
wp.waist.value = robot.dynamic.WT.value
wp.footLeft.value = robot.dynamic.LF.value
wp.footRight.value = robot.dynamic.RF.value
plug(robot.lfPosTrajGen.x, wp.footPositionLeft)
plug(robot.rfPosTrajGen.x, wp.footPositionRight)
plug(robot.waistToMatrix.sout, wp.waist)
plug(robot.lfToMatrix.sout, wp.footLeft)
plug(robot.rfToMatrix.sout, wp.footRight)
plug(robot.comTrajGen.x, wp.com)
plug(robot.comTrajGen.dx, wp.vcom)
plug(robot.comTrajGen.ddx, wp.acom)
robot.wp = wp
# --- Compute the values to use them in initialization
......
......@@ -41,23 +41,49 @@ robot.dynamic.WT.recompute(0)
# -------------------------- DESIRED TRAJECTORY --------------------------
# --- Trajectory generators
robot.comTrajGen = create_com_trajectory_generator(dt,robot)
robot.lfPosTrajGen = create_position_trajectory_generator(dt, robot, 'LF')
robot.rfPosTrajGen = create_position_trajectory_generator(dt, robot, 'RF')
# --- Walking pattern generator
# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
# robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
# robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
# --- Waist
robot.waistTrajGen = create_orientation_rpy_trajectory_generator(dt, robot, 'WT')
# robot.waistTrajGen.x.recompute(0) # trigger computation of initial value
robot.waistMix = Mix_of_vector("waistMix")
robot.waistMix.setSignalNumber(3)
robot.waistMix.addSelec(1, 0, 3)
robot.waistMix.addSelec(2, 3, 3)
robot.waistMix.default.value = [0.0]*6
robot.waistMix.signal("sin1").value = [0.0]*3
plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))
robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m')
plug(robot.waistMix.sout, robot.waistToMatrix.sin)
# --- Interface with controller entities
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
wp.omega.value = omega
wp.waist.value = robot.dynamic.WT.value
wp.footLeft.value = robot.dynamic.LF.value
wp.footRight.value = robot.dynamic.RF.value
plug(robot.lfPosTrajGen.x, wp.footPositionLeft)
plug(robot.rfPosTrajGen.x, wp.footPositionRight)
plug(robot.waistToMatrix.sout, wp.waist)
plug(robot.lfToMatrix.sout, wp.footLeft)
plug(robot.rfToMatrix.sout, wp.footRight)
plug(robot.comTrajGen.x, wp.com)
plug(robot.comTrajGen.dx, wp.vcom)
plug(robot.comTrajGen.ddx, wp.acom)
robot.wp = wp
# --- Compute the values to use them in initialization
......
......@@ -31,6 +31,8 @@ if c:
print("Executing the sinusoid...")
runCommandClient('robot.comTrajGen.startSinusoid(1,0.025,2.0)')
print("Sinusoid started!")
else:
print("Not executing the sinusoid")
c3 = ask_for_confirmation("Put the robot back?")
if c3:
......@@ -57,7 +59,7 @@ if c:
if c2:
print("Raising the foot...")
runCommandClient('h = robot.dynamic.LF.value[2][3]')
runCommandClient('robot.lfPosTrajGen.move(2,h+0.05,10.0)')
runCommandClient('robot.lfTrajGen.move(2,h+0.05,10.0)')
sleep(10.0)
print("Foot has been raised!")
c3 = ask_for_confirmation("Put the foot back?")
......@@ -67,7 +69,7 @@ if c:
if c3:
print("Putting the foot back...")
runCommandClient('robot.lfPosTrajGen.move(2,h,10.0)')
runCommandClient('robot.lfTrajGen.move(2,h,10.0)')
sleep(10.0)
print("The foot is back in position!")
c4 = ask_for_confirmation("Put the robot back?")
......
......@@ -21,6 +21,8 @@ if c:
print("Executing the sinusoid...")
runCommandClient('robot.comTrajGen.startSinusoid(1,0.025,2.0)')
print("Sinusoid started!")
else:
print("Not executing the sinusoid")
c3 = ask_for_confirmation("Put the robot back?")
if c3:
......@@ -47,7 +49,7 @@ if c:
if c2:
print("Raising the foot...")
runCommandClient('h = robot.dynamic.LF.value[2][3]')
runCommandClient('robot.lfPosTrajGen.move(2,h+0.05,10.0)')
runCommandClient('robot.lfTrajGen.move(2,h+0.05,10.0)')
sleep(10.0)
print("Foot has been raised!")
c3 = ask_for_confirmation("Put the foot back?")
......@@ -57,7 +59,7 @@ if c:
if c3:
print("Putting the foot back...")
runCommandClient('robot.lfPosTrajGen.move(2,h,10.0)')
runCommandClient('robot.lfTrajGen.move(2,h,10.0)')
sleep(10.0)
print("The foot is back in position!")
c4 = ask_for_confirmation("Put the robot back?")
......
......@@ -36,7 +36,7 @@ namespace dynamicgraph
//Size to be aligned "-------------------------------------------------------"
#define PROFILE_DUMMYWALKINGPATTERNGENERATOR_DCM_COMPUTATION "DummyWalkingPatternGenerator: dcm computation "
#define INPUT_SIGNALS m_omegaSIN << m_footLeftSIN << m_footRightSIN << m_waistSIN << m_footPositionLeftSIN << m_footPositionRightSIN << m_comSIN << m_vcomSIN << m_acomSIN << m_zmpSIN << m_referenceFrameSIN
#define INPUT_SIGNALS m_omegaSIN << m_footLeftSIN << m_footRightSIN << m_waistSIN << m_comSIN << m_vcomSIN << m_acomSIN << m_zmpSIN << m_referenceFrameSIN
#define INNER_SIGNALS m_rfSINNER
......@@ -59,8 +59,6 @@ namespace dynamicgraph
, CONSTRUCT_SIGNAL_IN(footLeft, MatrixHomogeneous)
, CONSTRUCT_SIGNAL_IN(footRight, MatrixHomogeneous)
, CONSTRUCT_SIGNAL_IN(waist, MatrixHomogeneous)
, CONSTRUCT_SIGNAL_IN(footPositionLeft, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(footPositionRight, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(com, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(vcom, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(acom, dynamicgraph::Vector)
......@@ -72,8 +70,8 @@ namespace dynamicgraph
, CONSTRUCT_SIGNAL_OUT(acomDes, dynamicgraph::Vector, m_acomSIN << m_rfSINNER)
, CONSTRUCT_SIGNAL_OUT(dcmDes, dynamicgraph::Vector, m_omegaSIN << m_comDesSOUT << m_vcomDesSOUT)
, CONSTRUCT_SIGNAL_OUT(zmpDes, dynamicgraph::Vector, m_omegaSIN << m_comDesSOUT << m_acomDesSOUT << m_rfSINNER << m_zmpSIN)
, CONSTRUCT_SIGNAL_OUT(footLeftDes, MatrixHomogeneous, m_footLeftSIN << m_footPositionLeftSIN << m_rfSINNER)
, CONSTRUCT_SIGNAL_OUT(footRightDes, MatrixHomogeneous, m_footRightSIN << m_footPositionRightSIN << m_rfSINNER)
, CONSTRUCT_SIGNAL_OUT(footLeftDes, MatrixHomogeneous, m_footLeftSIN << m_rfSINNER)
, CONSTRUCT_SIGNAL_OUT(footRightDes, MatrixHomogeneous, m_footRightSIN << m_rfSINNER)
, CONSTRUCT_SIGNAL_OUT(waistDes, MatrixHomogeneous, m_waistSIN << m_rfSINNER)
, m_initSucceeded(false)
{
......@@ -243,8 +241,6 @@ namespace dynamicgraph
}
MatrixHomogeneous footLeft = m_footLeftSIN(iter);
if(m_footPositionLeftSIN.isPlugged())
footLeft.translation() = m_footPositionLeftSIN(iter);
const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
......@@ -262,8 +258,6 @@ namespace dynamicgraph
}
MatrixHomogeneous footRight = m_footRightSIN(iter);
if(m_footPositionRightSIN.isPlugged())
footRight.translation() = m_footPositionRightSIN(iter);
const MatrixHomogeneous & referenceFrame = m_rfSINNER(iter);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment